ROS Pozyx Simulation

This package is to simulate the UWB range measurements in ROS environment. Generated data published to “uwb_data_topic”

UWB Anchors Add

The file that contains the UWB anchor information is in "launch/uwb_anchors_set.launch" file. It is possible to add or remove the UWB anchors as you like.
Note : uwb tag frame name should be as follows uwb_anchor_0, uwb_anchor_1, uwb_anchor_2 ...

UWB Tag information

Location of the tag has been taken from the robot position so to use this information "modelstate_index" parameter (for turtlebot3 modelstate_index =2) which is in "launch/uwb_simulation_initializing.launch" file must set correctly depending on the robot model used. It is possible to find your own robot parameter in "gazebo/model_states" topic.

Initialize

Use the command below in terminal to start it with default settings

roslaunch pozyx_simulation uwb_simulation_initializing.launch

Publish Topic

Name of the publisher topic is "uwb_data_topic". You can check it by using the command below in terminal

rostopic echo /uwb_data_topic

Message type consists of 3 different arrays
1. anchors name => int64[] destination_id
2. anchors distance to robot => float64[] distance
3. time stamp => time[] stamp

Map and Rviz

If you want to start manually with custom maps. You can change map and map configuration in maps folder and start manually with the codes below.

rosrun map_server map_server map.yaml rosrun rviz rviz rosrun pozyx_simulation uwb_simulation.py

Noise

Noise has been added to the every UWB ranging data

np.random.normal(0, uwb_dist*0.015,1)

How to use

Firstly you have to add your robot urdf file to this plugin This is important part because your real robot position publish with this plugin If you want to increase uwb freq you can change updateRate Or you want to add more than 1 robot you have to change topic name in here Also you can add in here noise but we are also add the in to the script

    <gazebo>
        <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>10.0</updateRate>
        <bodyName>base_link</bodyName>
        <topicName>ground_truth/state</topicName>
        <gaussianNoise>0.0</gaussianNoise>
        <frameName>world</frameName>
        <xyzOffsets>0 0 0</xyzOffsets>
        <rpyOffsets>0 0 0</rpyOffsets>
        </plugin>
    </gazebo>

For this process get robot or drone real position afther that to place uwb anchor in maps.

go repo folder /launch/uwb_anchors_set.launch and open
for each uwb anchor's set a name and position for example !Note : If you want to use more than 4 anchor names should be uwb_ancohor_(n+1) type of this.

<launch>
   <node pkg="tf" type="static_transform_publisher" name="uwb_anchor_0x6e36" args="1.4 2.0 0.0 0 0 0 1 map uwb_anchor_0 100" />
   <node pkg="tf" type="static_transform_publisher" name="uwb_anchor_0x6e33" args="-1.4 2.0 0.0 0 0 0 1 map uwb_anchor_1 100" />
   <node pkg="tf" type="static_transform_publisher" name="uwb_anchor_0x6e49" args="1.4 -2.0 0.0 0 0 0 1 map uwb_anchor_2 100" />   
   <node pkg="tf" type="static_transform_publisher" name="uwb_anchor_0x6e30" args="-1.4 -2.0 0.0 0 0 0 1 map uwb_anchor_3 100" />
</launch>

Now pack is ready to run.

Firstly call anchors roslaunch pozyx_simulation uwb_anchors_set.launch
After that run main calculation script `rosrun pozyx_simulation uwb_simulation.py

For check all pack is working
rostopic echo /uwb_data_topic

This process give robot to uwb_anchor distance for example

destination_id: [26955, 26952, 26959, 26954]
distance: [4192.039813830616, 2564.0268225317145, 3703.2810901751322, 1621.13755039097]
stamp: 
  - 
    secs: 1039
    nsecs: 440000000
  - 
    secs: 1039
    nsecs: 440000000
  - 
    secs: 1039
    nsecs: 440000000
---

This simulation give only robot to anchors distance. For calculate robot position use ieuagv_localization repo https://github.com/bekirbostanci/ieuagv_localization

Run

export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_gazebo turtlebot3_world.launch 

export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_navigation turtlebot3_navigation.launch 

roslaunch pozyx_simulation uwb_anchors_set.launch 

rosrun pozyx_simulation uwb_simulation.py 

rosrun advoard_localization kalman_filter_localization.py

Yu1107 / pozyx_simulation