Unable to load a saved FUNMAP map

Ask
1

Hello,

I’m trying to load a map I made using FUNMAP by running the following command:

roslaunch stretch_funmap mapping.launch map_yaml:="/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi.yaml"

However, I’m encountering an error. Here’s the error message:

Node funmap received first point cloud, so continuing.
HeadScan.from_file: base_filename = /home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi.yaml
Traceback (most recent call last):
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1386, in <module>
    node.main()
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1282, in main
    self.merged_map = ma.HeadScan.from_file(self.map_filename)
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/src/stretch_funmap/mapping.py", line 483, in from_file
    with open(base_filename + '.yaml', 'r') as fid:
FileNotFoundError: [Errno 2] No such file or directory: '/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi.yaml.yaml'
[funmap-11] process has died [pid 10656, exit code 1, cmd /home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap --load_map /home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi.yaml __name:=funmap __log:=/home/hello-robot/.ros/log/fcb12c6c-0b04-11ee-89d3-414212bcc179/funmap-11.log].
log file: /home/hello-robot/.ros/log/fcb12c6c-0b04-11ee-89d3-414212bcc179/funmap-11*.log

I attempted to remove the .yaml extension from the command, but it resulted in a different error. Here’s the new error message:

Node funmap received first point cloud, so continuing.
HeadScan.from_file: base_filename = /home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi
data = {'camera_depth_image_filename': '/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi_camera_depth.png', 'image.dtype': 'uint8', 'image.shape': [2667, 2667], 'image_filename': '/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi_image.npy.gz', 'image_origin': [0.012, 15.988, 0.0], 'm_per_height_unit': 0.0046456692913385824, 'm_per_pix': 0.006, 'np.max(image)': None, 'rgb_image_filename': '/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi_rgb.png', 'transform_corrected_to_original': None, 'transform_original_to_corrected': None, 'visualization_filename': '/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi_visualization.png', 'voi_data': {'axes': [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], 'frame_id': 'map', 'origin': [-8.0, -8.0, -0.05], 'x_in_m': 16.0, 'y_in_m': 16.0, 'z_in_m': 1.18}}
Traceback (most recent call last):
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1386, in <module>
    node.main()
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 1282, in main
    self.merged_map = ma.HeadScan.from_file(self.map_filename)
  File "/home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/src/stretch_funmap/mapping.py", line 487, in from_file
    max_height_image_base_filename = data['max_height_image_base_filename']
KeyError: 'max_height_image_base_filename'
Warning: Rate of calls to Pimu:trigger_motor_sync rate of 86.501897 above maximum frequency of 80.00 Hz. Motor commands dropped: 2
[funmap-11] process has died [pid 11482, exit code 1, cmd /home/hello-robot/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap --load_map /home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431_mhi __name:=funmap __log:=/home/hello-robot/.ros/log/dd4ef420-0b05-11ee-89d3-414212bcc179/funmap-11.log].
log file: /home/hello-robot/.ros/log/dd4ef420-0b05-11ee-89d3-414212bcc179/funmap-11*.log

I also tried using the following command, but I faced the same errors:

$ roslaunch stretch_funmap mapping.launch map_yaml:=$HELLO_FLEET_PATH/debug/merged_maps/<insert_map_filename>.yaml

Can anyone provide insights or suggestions on how to resolve this issue? I am using a Stretch 2 robot.

2 Likes
2

Hey @tony_rob, welcome to the forum! FUNMAP creates a few files when it saves a map. They are:

  • merged_map_<timestamp>.yaml - When loading a map, this is the file to give. It contains matrices that define where the map and base_link frames reside with respect to the max height image (mhi). It also references the mhi’s YAML.
  • merged_map_<timestamp>_mhi.yaml - This is the mhi YAML, defining type/units for the mhi image. It references four images.
  • merged_map_<timestamp>_mhi_image.npy.gz - This is the actual mhi image, saved as a numpy matrix.
  • merged_map_<timestamp>_mhi_rgb.png - This is the RGB component of the mhi image, saved as a png.
  • merged_map_<timestamp>_mhi_camera_depth.png - This is the depth component of the mhi image, saved as a png.
  • merged_map_<timestamp>_mhi_visualization.png - This is an optional visualization of the depth component, scaled to be easier to understand.

In your command above, you’re loading the merged_map_<timestamp>_mhi.yaml, instead of the merged_map_<timestamp>.yaml. So the command should be:

roslaunch stretch_funmap mapping.launch map_yaml:=/home/hello-robot/stretch_user/debug/merged_maps/merged_map_20230614163431"

One more thing, FUNMAP doesn’t know where the robot is located with respect to the map when the map is loaded, so it’s common to perform a global localization at the beginning. This performs a head scan, and you’ll see the robot jump in Rviz to it’s pose in the map.

rosservice call /funmap/trigger_global_localization "{}"
1 Like
3

Hi @bshah !

Thanks for your prompt reply and explanation! I just tried and I was able to open the map.

Best regards,
Tony

4

@bshah I tried to navigate in the map but I am getting a warning plus my robot is not able to navigate beyond three planned waypoints for some reason. if I want it to navigate a relatively long distance and it plans more than 3 waypoints it will get stuck in the third one. This is the warning and error I am getting:

[ WARN] [1686950616.027990555]: Could not get base to fixed frame transform, Lookup would require extrapolation 0.122861985s into the future.  Requested time 1686950615.803190470 but the latest data is at time 1686950615.680328608, when looking up transform from frame [base_link] to frame [odom]
[ERROR] [1686950616.118106]: /stretch_driver joint_traj action: Time to execute the current goal point = <positions: [1.3136445879578813]
velocities: []
accelerations: []
effort: []
time_from_start:
  secs: 0
  nsecs:         0> exceeded the default_goal_timeout = 10.0

[INFO] [1686950616.185930]: Failed to reach forward motion goal.
[ERROR] [1686950616.206311]: Failed to reach forward motion goal.

I guess the error is due the timeout to find a goal, which seems weird because the trajectories that its planning aren’t close from any obstacles. Do you know where I can modify this parameter ? However, I am unsure about the warning’s origin.

Best,
Tony

5

@tony_rob, the Failed to reach forward motion goal printout is actually the important error here. It indicates the mobile base attempted to achieve a certain amount of forward translation multiple times, but failed to achieve it. It’s a bug that I’ve personally experienced when the base is translating on carpet or other flooring where the wheels can slip.

Let me know if you’d like help figuring out this bug; I’d be happy to dig deeper. If you feel comfortable with the FUNMAP codebase, I’d recommend adding the forward translation goal to the error printout, and then attempt to command the same goal using Stretch Body, because with the Python API, you have access to the trace toolkit which can help you debug by showing you what the robot is seeing.