New Environment Workflow

This page describes how to move the current FYP system to a new place and rebuild the maps, floor data, and navigation configuration needed for multi-floor deployment.

1. Purpose

This page is not about replaying the already prepared Life and Science Building route. It is about what to do when the robot is taken to a new building or a new area and the current maps, stairs, and waypoints are no longer valid.

The overall job is:

1. record a new point cloud map
2. move that point cloud into the localization pipeline
3. create one 2D map for each floor
4. record stairs and goal poses
5. update multi-floor configuration files
6. verify that the robot can still localize, climb stairs, and reach the goal

2. Save A New PCD Map With FAST-LIVO2

The first step is to record a new .pcd map for the environment.

Open the FAST-LIVO2 MID-360 configuration:

• FAST-LIVO2/config/MID360.yaml

Enable point cloud saving:

pcd_save:
  pcd_save_en: true
  colmap_output_en: false
  filter_size_pcd: 0.15
  interval: -1

The key change is:

pcd_save_en: true

Record the new environment with the robot. Try to cover:

• the walking corridors
• stair entrances and exits
• turning points
• the final goal area
• any narrow passages that will affect navigation

If the map coverage is poor, the later localization and 2D floor extraction will also be poor.

After the required .pcd file is produced, set pcd_save_en back to false if you do not want FAST-LIVO2 to keep saving large files during normal operation.

3. Move The PCD Into hdl_localization

The new .pcd file is not only for map generation. It must also be used by hdl_localization as the global point cloud map.

Move the generated .pcd file into the data location used by the localization package. For example:

cp /path/to/new_environment.pcd /path/to/FYP_ws/src/fyp_src/hdl_localization/data/

Then update the localization configuration or launch path so that hdl_localization loads the new map instead of the old one.

Before continuing, confirm:

• the new .pcd is in the expected hdl_localization data directory
• the localization launch path points to this new .pcd
• no old map path is still being loaded by mistake

If this step is skipped, the robot may still try to localize against the previous environment.

4. Generate A 2D Grid Map For Each Floor

After the 3D point cloud map is ready, generate one 2D grid map for each floor with:

• pcd2grid.py

The script currently uses these main parameters:

pcd_path = "/home/nvidia/catkin_ws/src/hdl_localization/data/LS_123_raw_points.pcd"
map_res = 0.02
z_min = 4.9
z_max = 5.2

For a new environment, update at least:

• pcd_path
• z_min
• z_max

Change pcd_path so it points to the new .pcd placed under hdl_localization.

Change z_min and z_max so that only one floor is extracted at a time. This is the critical step for multi-floor deployment. If the height range is wrong, the map may include ceiling points, another floor, or mixed stair geometry.

If needed, also adjust:

• map_res

Run the script, inspect the published /map, and then save the result:

rosrun map_server map_saver -f my_floor

Repeat this process once per floor. The final result should be one .pgm and one .yaml for each floor.

Move or rename the generated map files into the map directory used by the navigation manager:

• navigation_manager/maps

Use consistent names, because these filenames are referenced later by waypoints.yaml.

5. Inspect Floor Maps In RViz

After each 2D floor map is generated, open it in RViz and inspect the important navigation locations.

Use RViz to examine:

• stair entrance positions
• stair exit positions
• mission start area
• final goal area
• the heading direction at each important point

The practical goal is to collect pose values in this format:

[x, y, z, qx, qy, qz, qw]

Use 2D Nav Goal / move_base_simple/goal while inspecting the map so you can read and record the pose positions and orientations for:

• each stair connection point
• each floor goal
• any future special waypoint you may need

6. Update waypoints.yaml

The core multi-floor mission configuration is:

• navigation_manager/config/waypoints.yaml

This file must be updated to match the new environment.

The main parts are:

floors:
  0:
    map_name: "..."
    stairs:
      s0: [x, y, z, qx, qy, qz, qw]
      s1: [x, y, z, qx, qy, qz, qw]
    goals:
      office_goal: [x, y, z, qx, qy, qz, qw]

For each floor:

• update map_name
• update each stair pose under stairs
• update the target pose under goals

Also update the cross-floor edges:

stair_edges:
  - from_floor: 0
    from_stair: s0
    to_floor: 1
    to_stair: s1
    z_offset: ...

This section tells the system how floors connect and which stair points are paired across floors.

7. Determine z_offset

The multi-floor logic also depends on a correct height offset.

In the current implementation, z_offset is part of the stair edge definition and is also related to the floor offset settings:

stair_edges:
  - from_floor: 0
    from_stair: s0
    to_floor: 1
    to_stair: s1
    z_offset: -4.58

settings:
  floor_z_offsets:
    0: 0.0
    1: -4.58
    2: -8.86

You must determine a correct z_offset for each floor transition in the new environment.

If this value is wrong:

• localization may drift after floor switching
• map switching may not match the robot height assumption
• the robot may navigate on the wrong floor state

So after measuring or validating the new height relationship between floors, update:

• stair_edges[*].z_offset
• settings.floor_z_offsets

8. Update Launch Parameters

The initial floor map and mission target are configured in:

• navigation_manager/launch/start_nav_logic.launch

The important parameters are:

<arg name="map_file" default="$(find navigation_manager)/maps/LS_123_floor0.yaml"/>
<param name="initial_floor" value="0"/>
<param name="initial_target_floor" value="2"/>
<param name="initial_target_key" value="office_goal"/>

For a new environment, update these items:

• map_file
• initial_floor
• initial_target_floor
• initial_target_key

These values must match:

• the actual first floor used at startup
• the new map filenames
• the goal name written in waypoints.yaml

The final runtime command still comes through:

• robot_bringup/launch/1.launch

So if start_nav_logic.launch still points to the old floor or old goal, the runtime behavior will also stay tied to the old environment.

9. Recheck Stair Strategy

Map replacement alone is not enough. The stair climbing strategy itself must still match the new building.

In the current implementation, stair climbing behavior is managed through:

• navigation_manager/scripts/stair_climber.py

You should confirm whether the new stairs match the current strategy.

If the new building uses a U-shaped staircase, the current stair strategy may not be sufficient. In that case, refer to the historical implementation in your FYP repository:

• commit 17b344c
• climb_stair.py from that commit

This should be treated as the reference version for the U-shaped stair strategy.

Also recheck the stair-related launch parameters in:

• navigation_manager/launch/start_nav_logic.launch

Relevant parameters include:

• half_height
• full_height
• platform_forward_duration
• top_exit_duration

If the new stairs differ in height, landing shape, or exit behavior, these parameters may also need adjustment.

10. Validation Checklist

Before treating the new environment deployment as complete, verify all of the following:

• the new .pcd was recorded successfully
• hdl_localization is loading the new point cloud map
• every floor has its own valid 2D .pgm/.yaml map
• waypoints.yaml has been updated for stairs and goals
• z_offset values are updated for the new floor transitions
• start_nav_logic.launch points to the correct initial map and target floor
• the stair climbing strategy matches the real staircase shape
• localization remains stable after floor switching
• the robot can finally reach the intended target in the new environment

This is the minimum full redeployment path for moving the FYP system to a new place.