search

MigVega / Ogm2Pgbm

Robust BIM-based 2D-LiDAR Localization for Lifelong Indoor Navigation in Changing and Dynamic Environments
https://publications.cms.bgu.tum.de/2022_ECPPM_Vega.pdf
MIT License
175 stars 29 forks source link

Pgm2Posegraph in ROS2 #8

Closed LihanChen2004 closed 5 months ago

LihanChen2004 commented 5 months ago

Pgm2Posegraph in ROS2

This might also be a way to edit the posegraph map, but it's a bit convoluted.

I. Refine and Edit the PGM Map

Modify the map as needed by adding or removing barriers. The most crucial step is to completely black out the obstacles, ensuring there are no white areas within any obstacles!

Before After
截图+2024-05-09+13-30-02 image

II. Use Ogm2Pgbm to Obtain a Rosbag with PointCloud and Poses

GitHub - MigVega/Ogm2Pgbm: Robust BIM-based 2D-LiDAR Localization for Lifelong Indoor Navigation in Changing and Dynamic Environments

This repository implements the conversion of Occupancy Grid Maps (OGM) to Pose Graph-based maps. The workflow is as follows:

  1. Subscribe to map data, which can be from a map topic.
  2. Skeletonize the map and obtain its Voronoi polygon path points.
  3. Perform coverage path planning on the path points, including:
    • Extracting the farthest endpoint pair as start and end points.
    • Dilating the map with a 2x2 kernel to thicken the centerline.
    • Using a wavefront algorithm in coverage path planning to get sorted path points.
  4. Perform ray tracing on the path points sequentially and publish the /laserscan topic.

2.1 Use Docker to Obtain the Package

The original repository uses the ROS1 melodic version.

I have prepared a Docker image for easy deployment.

  1. Pull the Docker image:

    docker pull lihanchen2004/ogm2pgbm:latest

  2. Clone the repository. The /Ogm2Pgbm/workspace/map/ directory will be mapped to the Docker container, making it easy to replace the map:

    git clone https://github.com/LihanChen2004/Ogm2Pgbm
cd Ogm2Pgbm

  3. Create the container based on lihanchen2004/ogm2pgbm:latest:

    ./autorun.sh

2.2 Start the Package

Copy the .pgm and .yaml files you need to convert to the Ogm2Pgbm/workspace/map/ directory on the host machine.

All following operations should be performed inside the Docker container. Ensure the current terminal is within the Docker container.

MAP_NAME=RMUC

roslaunch ogm2pgbm ogm2pgbm.launch map_file:=/root/workspace/map/$MAP_NAME.yaml record:=true

截图+2024-05-09+13-53-08

Wait for 2-3 minutes. After completion, the program will output "Done" in the terminal.

截图+2024-05-09+13-55-04

Terminate the program with Ctrl+C. The rosbag will automatically be saved to /root/.ros/ogm2pgbm_sensordata.bag.

2.3 Download the Rosbag to the Host Machine

All following operations should be performed on the host machine.

  1. Get the CONTAINER ID:

    docker ps

  2. Save the rosbag to the host machine:

    Remember to modify CONTAINER_ID! Here, I save the rosbag from the container to the host's Download directory:

    CONTAINER_ID=cb06e4ece104
CONTAINER_PATH=/root/.ros/ogm2pgbm_sensordata.bag
DST_PATH=~/Downloads/

docker cp $CONTAINER_ID:$CONTAINER_PATH $DST_PATH

III. Convert .bag to .db3

All following operations should be performed on the host machine.

In ROS1, rosbag files have a .bag suffix, which is a binary format for storing ROS messages. ROS2 has improved and extended the rosbag format, adopting an SQLite-based database format that includes a .db3 database and a .yaml file.

Ternaris / rosbags · GitLab

  1. Install the conversion package:

    pip install rosbags

  2. Navigate to the directory containing the rosbag and execute the following command to convert ogm2pgbm_sensordata.bag to RMUC.db3:

    rosbags-convert --src ogm2pgbm_sensordata.bag \
--dst RMUC.db3 \
--src-typestore empty \
--dst-typestore ros2_humble \
--exclude-topic /rosout /robot/map /rosout_agg

IV. Play the Rosbag and Generate the Map

All following operations should be performed on the host machine.

4.1 Preparation

You can use the following rviz configuration file for visualization.

ogm2pgbm.rviz (Click to expand) ```yaml Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 Tree Height: 549 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /map_updates Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /scan Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true robot_base_link: Value: true robot_map: Value: true robot_odom: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: robot_map: robot_base_link: {} robot_odom: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: robot_base_link Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Covariance x: 0.25 Covariance y: 0.25 Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Distance: 25.282127380371094 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: -1.1839663982391357 Y: 1.0030579566955566 Z: -0.871300458908081 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.1747967004776 Target Frame: Value: Orbit (rviz) Yaw: 3.125408411026001 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000438000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b30000003efc0100000002fb0000000800540069006d00650100000000000004b3000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000357000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1203 X: 1813 Y: 226 ```

4.2 Start

  1. Start the slam_toolbox to prepare for mapping:

    ros2 run slam_toolbox async_slam_toolbox_node --ros-args \
-p use_sim_time:=True \
-p odom_frame:=robot_odom \
-p base_frame:=robot_base_link \
-p map_frame:=robot_map \
-p do_loop_closing:=False \
-p max_laser_range:=10.0

  2. Play the rosbag:

    ros2 bag play RMUC.db3

  3. Start rviz2 for visualization:

    Ensure to modify the rviz2 configuration file path:

    rviz2 -d ~/Downloads/ogm2pgbm.rviz

    截图+2024-05-09+14-19-23

4.3 Save the .posegraph Map

After the rosbag finishes playing, set the filename in the SlamToolBoxPlugin on the left side of the rviz2 interface, and click Serialize Map to save the .posegraph+.data map. Click Save Map to save the .pgm+.yaml map.

Note

The reason for not using Ogm2Pgbm to directly generate the .posegraph map is due to a previously encountered issue where maps generated in ROS1 could not be loaded in ROS2. This is likely due to a version issue with slam_toolbox.

[localization_slam_toolbox_node-12] [ERROR] [1715064875.276643345] [slam_toolbox]: serialization::Read: Failed to read file: Exception: unregistered class
[localization_slam_toolbox_node-12] [ERROR] [1715064875.276682895] [slam_toolbox]: DeserializePoseGraph: Failed to read file: /home/lihanchen/NAVIGATION_WS/RM2024_SMBU_auto_sentry_ws/install/rm_bringup/share/rm_bringup/map/RMUC.
MigVega commented 5 months ago

Hi Lihan, Great Work! Basically you adopted the package to work with ROS2 and provide comprehensive step by step! Feel free to create a pull request, even if it's only in the readme of the repository. I believe this can be very helpful for the community. Thank you.

LihanChen2004 commented 5 months ago

Hi Lihan, Great Work! Basically you adopted the package to work with ROS2 and provide comprehensive step by step! Feel free to create a pull request, even if it's only in the readme of the repository. I believe this can be very helpful for the community. Thank you.

Thank you for your recognition! I will submit a Pull Request recently. I will try to retain and merge the original Running The Code section as much as possible.