Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

indoor_localization Tutorial

Installation

To install the package, please run the following commands in terminal:

$ cd %YOUR_WORKSPACE_NAME%/src
$ git clone -b kinetic-devel https://github.com/inomuh/indoor_localization
$ catkin_make

Before run the package, 3rd party library Shapely must be installed. To install this library, run the following command in terminal:

$ pip install shapely

Configuration

Configure localization_params.yaml

• 1. localization_mode: Please set this parameter according to the dimension you will work with. It takes 3 different integer values:

  • 1 - localization in one dimensional (1D)

  • 2 - localization in two dimensional (2D)

  • 3 - localization in three dimensional (3D)

• 2. sig_c: Standard deviation of TDOA measurements. Default value is 0.0625 in float.

• 3. rate: Refreshing rate of subscribers/publishers in Hz. Default value is 5.

• 4. tag_z: If localization mode selected as 2D, you should set this parameter as height of tag. Height of tag must be in meter.

• 5. thr: This parameter (threshold) indicates the maximum distance between two positions (pre- and next-position). If the distance between positions is less than the threshold value, it means the tag does not move or vice versa. (If the tag is moving in 2D environment, change this value rationally.)

• 6. period_sec: Refreshing value of KPI parameters in seconds. If you want to refresh the KPI parameters in 8 hours, this parameter must be 28800 seconds (8*60*60).

• 7. period_nsec: Refreshing value of KPI parameters in nano-seconds. Default value is 0 nsecs.

• 8. initial_Tx: x coordinate of starting position of tag. (e.g., if the starting position of tag selected as [10, 15, 1.1], this parameter must be set as 10.)

• 9. initial_Ty: y coordinate of starting position of tag. (e.g., if the starting position of tag selected as [10, 15, 1.1], this parameter must be set as 15.)

• 10. initial_Tz: z coordinate of starting position of tag. (e.g., if the starting position of tag selected as [10, 15, 1.1], this parameter must be set as 1.1.)

• 11. L1: If localization mode selected as 1D, this parameter indicates the starting point of line where tag moves. This parameter must be in a list (e.g., [12, 8, 0.5]).

• 12. L2: If localization mode selected as 1D, this parameter indicates the end point of line where tag moves. This parameter must be in a list (e.g., [16, 8, 0.5]).

Configure region_params.yaml

In region_params.yaml, region names and their coordinates must be set. Regions can be selected as any polygons like triangle, rectangle, pentagon etc.

But the most important thing in here is, the coordinate values of polygons must be entered in the parameter such that the corners of the polygon follow one another.

For example, you have two regions and they are "hallway" - rectangle and "warehouse" - pentagon:

   1 regions:
   2     hallway: [               # it is a rectangle
   3         [0.0, 0.0, 10.0],    # 1st corner
   4         [8.0, 0.0, 10.0],    # 2nd corner
   5         [8.0, 2.5, 10.0],    # 3rd corner
   6         [0.0, 2.5, 10.0]     # 4th corner
   7     ]
   8 
   9     warehouse: [             # it is a pentagon
  10         [12.0, 5.0, 10.0],   # 1st corner
  11         [17.0, 5.0, 10.0],   # 2nd corner
  12         [20.0, 7.5, 10.0],   # 3rd corner
  13         [14.5, 10.0, 10.0],  # 4th corner
  14         [9.0, 7.5, 10.0]     # 5th corner
  15     ]

How should you publish your own AnchorScan data?

After making sure that all parameters are set optimally, in this section you will learn how to publish TDOA values through the AnchorScan message. First let's talk about the AnchorScan message:

AnchorScan.msg

AnchorScan.msg is a message type which holds the fixed anchors' (UWB sensors) informations and Time Difference of Arrival (TDOA) values. These values are used by calculate the position, anchor selection, KPI parameter calculation and error estimation.

Here is the AnchorScan.msg:

Header header

int32[] AnchorID             # ID of fixed sensors.

float64[] x                  # x coordinates of fixed sensors respectively in meter
float64[] y                  # y coordinates of fixed sensors respectively in meter
float64[] z                  # z coordinates of fixed sensors respectively in meter

float64[] tdoa_of_anchors    # TDOA values of UWB signals according to sensor with minimum ID.

Let's examine the message line by line:

Header header

Header message includes the time stamp. In your AnchorScan data publisher code, you should publish the ROS time.

int32[] AnchorID             # ID of fixed sensors.

AnchorID holds the ID's of the anchors (UWB sensors). In your AnchorScan data publisher code, you should publish the IDs of anchors in integer type list.

float64[] x                  # x coordinates of fixed sensors respectively in meter
float64[] y                  # y coordinates of fixed sensors respectively in meter
float64[] z                  # z coordinates of fixed sensors respectively in meter

x, y and z values holds the sensors' positions respectively with sensor IDs. In your AnchorScan data publlisher code, you should publish the coordinates of anchors respectively with anchor IDs.

float64[] tdoa_of_anchors    # TDOA values of UWB signals according to sensor with minimum ID.

Most important thing about this package is publishing the TDOA data properly. (More deteails will be given in next step.)

Write Your Own UWB Hardware ROS Driver

• Create a hardware package

First create a ROS driver package named "uwb_hardware_driver" in your workspace:

$ cd %YOUR_WORKSPACE_NAME%/src
$ catkin_create_pkg uwb_hardware_driver std_msgs rospy roscpp message_generation
$ cd ..
$ catkin_make

• Create AnchorScan message

After that create a msg file in your package:

$ cd src/uwb_hardware_driver
$ mkdir msg

Go into the msg file and create an empty document named as "AnchorScan.msg". In AnchorScan.msg file, copy these lines into it:

# This is a message to hold the fixed anchors' (UWB sensors) information at known coordinates
# and Time Difference of Arrival (TDOA) values.
#
# The indoor positioning systems developed using UWB signals include multiple sensors
# that transmit UWB signals in an environment.
#
# Calculations such as position, anchor selection etc. are made by using TDOA measurements 
# from these sensors and coordinate information of the sensors.
#
# With the defined AnchorScan message, it allows the publish of this information
# from the sensors via any developed firmware.

Header header

int32[] AnchorID             # ID of fixed sensors.

float64[] x                  # x coordinates of fixed sensors respectively in meter
float64[] y                  # y coordinates of fixed sensors respectively in meter
float64[] z                  # z coordinates of fixed sensors respectively in meter

float64[] tdoa_of_anchors    # TDOA values of UWB signals according to sensor with minimum ID.

• Create hardware driver source code

After that create a src file in your package:

$ mkdir src

Go into the src file and create an empty document named as "hardware_ros_driver.py". This python file should contain your own driver's code.

You should find the template code of the driver in here. Please read the instructions in the code carefully!

• Edit CMakeLists.txt of hardware package

Open the CMakeLists.txt of "uwb_hardware_driver" package and copy these lines into it:

cmake_minimum_required(VERSION 2.8.3)
project(uwb_hardware_driver)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  message_generation
  std_msgs
)

find_package(PkgConfig REQUIRED)

 add_message_files(
   FILES
   AnchorScan.msg
 )

 generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
 )

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES uwb_hardware_driver
  CATKIN_DEPENDS roscpp rospy message_runtime std_msgs
#  DEPENDS system_lib
)

include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

• Edit package.xml of hardware package

Open the package.xml of "uwb_hardware_driver" package and add these lines into it:

<build_depend>message_generation</build_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>message_runtime</exec_depend>

Edit indoor_localization Package Considering Hardware ROS Package

Some modifications must be done in indoor_localization package in order to work in a coordination with the written hardware driver package.

Firstly, open the CMakeLists.txt document of indoor_localization and add dependencies:

generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
  uwb_hardware_driver
)

...

catkin_package(
  # INCLUDE_DIRS include
  LIBRARIES indoor_localization uwb_hardware_driver
  CATKIN_DEPENDS geometry_msgs message_generation roscpp rospy rostest uwb_hardware_driver
  DEPENDS system_lib
)

After that, open the package.xml document of indoor_localization and add these lines:

<build_depend>uwb_hardware_driver</build_depend>
<build_export_depend>uwb_hardware_driver</build_export_depend>
<exec_depend>uwb_hardware_driver</exec_depend>

Finally, add these line into the anchor_selection_node.py:

   1 from uwb_hardware_driver.msg import AnchorScan

Launch the package

After all the above steps (setting the parameters, prepare your own driver package etc.) have been completed for the indoor positioning system to work, let's move on how to launch the system.

First, we should get the AnchorScan data to work with the localization system. Open a new terminal and run the following commands:

$ cd %YOUR_WORKSPACE_NAME%/src
$ chmod +x uwb_hardware_driver/hardware_ros_driver.py
$ rosrun uwb_hardware_driver hardware_ros_driver.py

After, launch the indoor_localization package. Open a new terminal and run the following commands:

roslaunch indoor_localization start_localization.launch