Fusing Odometry, UWB Ranging, and Spatial Detections for Relative Multi-Robot Localization

This repo contains code for refine the Ultral-Wideband ranging with the detected spatial information of an object seen by multiple robots at the same time.

If you find the code or data in this repository useful in your research, the related paper is available in arXiv:

@article{xianjia2023loosely, 
    title="Loosely Coupled Odometry, {UWB} Ranging, and Cooperative Spatial Detection for Relative Monte-Carlo Multi-Robot Localization", 
    author="Xianjia Yu and  Iacopo Catalano and Paola {Torrico Mor\'on} and  Sahar Salimpour and Tomi Westerlund and Jorge {Pe\~na Queralta}", 
    journal="arXiv", 
    year="2023"
}

Installation

1. ROS 2 galactic
   Install ros2 galactic following the instruction here

2. Eclipse Zenoh
   Add Eclipse Zenoh private repository to the sources list:

    echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | sudo tee -a /etc/apt/sources.list > /dev/null
    sudo apt update
   

   Then either:
   install the plugin with: sudo apt install zenoh-plugin-dds.
   install the standalone executable with: sudo apt install zenoh-bridge-dds.

3. depthai_ros_msgs

    sudo apt install ros-foxy-depthai-ros-msgs
   

4. Others

    pip install numpy matplotlib pfilter
   

How to run

Please see below for instructions on running the proposed algorithms with predefined data.

Proposed Particle Filter Approach

Currently, if you only want to run one round of each filter. For multiple robots,

python3 pf_ros2_multi_ulv.py --fuse_group 0 --with_model False 

Arguments meaning(currently not used):

--fuse_group 
  0: only uwb
  1: uwb and vision

--with_model False  # enable the LSTM ranging error estimation

For single UWB range.

pf_ros2_single_ulv.py --fuse_group 0 --with_model False

After running this code, the images of particles will be saved in

../images/

The groudtruth and estimated relative pose will be in

../pos/

The errors will be in

../errors/

Triangulation

python3 tri_ros2_ul.py --with_model False

Results visualization

currently mainly run the code in

script/errors/

Single UWB range

APE	Trajectory

Multiple UWB ranges

APE

Trajectories

Single UWB range

APE	Trajectory

Multiple UWB ranges

APE

Trajectories

Acknowledgement

This research work is supported by the Academy of Finland’s AeroPolis project (Grant No. 348480), and by the R3Swarms project funded by the Secure Systems Research Center (SSRC), Technology Innovation Institute (TII).

To Do List

• Change the results in Readme
• Add the paper abstract and Arkiv link
• A None ROS version