l4ms/opil.sw.sp.central repository overview

⁠Sensing and Perception

The module Sensing and Perception (SP) is a software module as part of OPIL (Open Platform for Innovation in Logistics). It provides the pose of the AGV inside the built map of the environment in which the AGV is navigating and updates the map with the new sensor readings. Additionally, it can build the map with SLAM (Simultaneous Localization And Mapping) if no map is given initially. It uses the range data (laser, kinect) for map building and updating the map, and odometry sensors (encoders, IMU) with range data for localization inside the map.

There are two forms of SP module: the Central SP and the Local SP. This docker container is the Central SP.

Every AGV has it's own Local SP, that takes care of localization and mapping. It creates a local map of it's surroundings. Every AGV sends this local map data as an update to a Central SP, which is on the OPIL server. The Central SP creates a global map of the factory in the form of topology graph for Task Planner and HMI using all the local updates from AGVs.

⁠How to start the Central SP docker container

To start a docker container prepare a docker-compose.yml following this example for the local machine:

version: "3"
services:      
    #Context Broker
    orion:        
        image: fiware/orion
        ports:
            - 1026:1026
        command: 
            -dbhost mongo
    mongo:
        restart: always
        image: mongo:3.4
        command: --nojournal    
#S&P
    sp:
        restart: always
        image: l4ms/opil.sw.sp.central:latest
        volumes:
            #- path on the host : path inside the container
            - /tmp/.X11-unix:/tmp/.X11-unix:rw
            - ./annotations.ini:/annotations.ini:ro
            - ./floorplan.yaml:/map.yaml:ro
            - ./floorplan.png:/map.png:ro
            - ./topology.launch:/topology.launch:ro
        environment:
            - FIWAREHOST=orion
            - HOST=sp
            - DISPLAY=$DISPLAY
        ports: 
            - "39002:39002"

Then, start it from the folder where you put your docker-compose.yml file:

xhost local:root (call this only once - this is for display)
sudo docker-compose up

In windows OS you need to open command prompt and type docker-compose up from the folder where you saved docker-compose.yml. The display in windows OS does not work, so you will not be able to see visualizations of topic exchange in rviz, but you can see subscriptions and entities in a web browser. The following instructions are for linux OS, tested on Ubuntu 16.04.

This example uses the version 3 and it does not need links to enable services to communicate. To update the docker-compose to the working version for the version 3 (1.22) type: (NOTE: you should remove prior versions of docker-compose)

sudo curl -L https://github.com/docker/compose/releases/download/1.22.0/docker-compose-`uname -s`-`uname -m` -o /usr/local/bin/docker-compose

and then

chmod +x /usr/local/bin/docker-compose

To check the version type:

sudo docker-compose --version

You should see:

docker-compose version 1.22.0, build f46880fe

After starting the docker-compose you should see the entities in, e.g. firefox, at the address http://localhost:1026/v2/entities⁠. There should be a topic /map/graph.

This docker container starts the creation of a topology graph from the given map file and included annotations from the annotation file. As a default, no map is loaded and the following message will appear in terminal:

sp_1     | please insert a floorplan.png and floorplan.yaml file to begin!
sp_1     | in your docker-compose.yml put under volumes:
sp_1     |             - ./floorplan.yaml:/map.yaml:ro
sp_1     |             - ./floorplan.png:/map.png:ro
opilserver_sp_1 exited with code 0

⁠Setting the map⁠

Two files need to be prepared and put under volumes in docker-compose.yml:

• floorplan.yaml
• floorplan.png

Right now in this docker container only PNG file is supported. Export your floorplan layout to PNG. Then, you need to set up parameters for transforming the PNG file into a map with the origin and dimensions in meters. This needs to be prepared in the floorplan.yaml. Here is an example:

#floorplan.yaml
image: map.png
resolution: 0.0196
origin: [-12.7095, -7.5866, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

where

• the image name is map.png by which your floorplan.png is overwritten on the docker side
• resolution defines the size of the pixel of the png file in meters. To calculate the resolution, you need to know the width of the image in meters. Then, simply divide the width in meters with the width in the number of pixels.
• negate can inverse the free-occupied values if set to 1
• occupied_thresh and free_thresh define the thresholds that tell which brightness level should be considered as occupied and free, respectively.
• origin is the (x,y,z) vector in meters describing the lower-left corner. With (0,0,0) the origin will be in the lower-left corner.

The files floorplan.yaml and floorplan.png need to be in the folder where you put your docker-compose.yml. After restarting docker-compose.yml, i.e.,

sudo docker-compose up

In this example, blue squares are the vertices of the graph placed regularly in the grid (distanced 2 meters), and blue line segments are the edges of the graph. Red squares are possible vertex positions which are occupied. To zoom the rviz window use mouse scroll, to translate the view press shift key and mouse button at the same time.

⁠Setting the topology cell_size

It can be seen that here the topology nodes are too rare and we are missing some of them in narrow passages. In the following we will change the size of the grid cell so that we do not miss topology nodes in the passages between the racks (however, this will be solved differently in OPIL v3). In this example map the passage is 2.5 m wide, which means the size of the cell size should be half of it to include the worst case of the alignment of passages with the grid. To change the size of the grid cell for calculating the topology, prepare the topology.launch file:

⁠topology.launch⁠

<launch>
<node name="map_server" pkg="map_server" type="map_server" args="$(find maptogridmap)/launch/map.yaml" respawn="false" >
<param name="frame_id" value="/map" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find maptogridmap)/singlerobot.rviz" /> 
<node name="map2gm" pkg="maptogridmap" type="map2gm" required="true" output="screen">
        <param name="cell_size" type="double" value="1.0" />
        <param name="annotation_file" textfile="$(find maptogridmap)/launch/annotations.ini" />
</node>
    <!-- Run FIROS -->
    <node name="firos" pkg="firos" type="core.py" />
</launch>

where the cell_size is set to 1.0 m. To use the created topology.launch put it next to the docker-compose.yml file and restart docker-compose.yml.

⁠Setting the annotations

To use arbitrary annotations you should create annotations.ini file and put it in the same folder next to the docker-compose.yml:

#annotations.ini
[W1]
# coordinates
point_x = -5.4
point_y = -6.0
theta = 270
distance = 1.8

[W2]
# coordinates
point_x = -4.5
point_y = -6.0
theta = 270
distance = 1.8

[W3]
# coordinates
point_x = -3.6
point_y = -6.0
theta = 270
distance = 1.8

[MoldingPallet]
# coordinates
point_x = -6.0
point_y = 10.5
theta = 90
distance = 1.8

where

• W1, W2, W3, MoldingPallet are the annotation labels
• point_x, point_y are coordinates of the annotation
• theta and distance determine where the topology vertex should be so that Task Planner can use this coordinates as the goal distanced for a defined distance from the annotation and oriented towards the annotation so that AGV has heading theta with respect to the positive x-axis.

Coordinates of the annotations can be chosen arbitrarily. They can be set as the middle of the pallet or the corner of the pallet, for example. The most important is to calculate the right position where an AGV will be placed in front of the annotation by setting the right distance and orientation from the annotation coordinates.

After restarting docker-compose.yml, four annotations are in this example, which change the coordinates of vertices where the AGV needs to be located to perform some operation, e.g., loading/unloading.

⁠How to start the Local and Central SP docker containers together

Prepare the following docker-compose.yml to start both Central and Local SP. This example uses the same machine for Local and Central SP, but you can also start them on different machines. This example uses the Local SP that contains the Stage simulator. Check how to prepare the Local SP here⁠

version: "3"
services:      
    #Context Broker
    orion:        
        image: fiware/orion
        ports:
            - 1026:1026
        command: 
            -dbhost mongo
    mongo:
        restart: always
        image: mongo:3.4
        command: --nojournal    
#S&P
    sp:
        restart: always
        image: l4ms/opil.sw.sp.central:latest
        volumes:
            #- path on the host : path inside the container
            - /tmp/.X11-unix:/tmp/.X11-unix:rw
            - /tmp/.X11-unix:/tmp/.X11-unix:rw
            - ./annotations.ini:/annotations.ini:ro
            - ./floorplan.yaml:/map.yaml:ro
            - ./floorplan.png:/map.png:ro
            - ./topology.launch:/topology.launch:ro
        environment:
            - FIWAREHOST=orion
            - HOST=sp
            - DISPLAY=$DISPLAY
        ports: 
            - "39002:39002"
    splocal:
        restart: always
        image: l4ms/opil.iot.sp.local:latest
        volumes:
            #- path on the host : path inside the container
            - /tmp/.X11-unix:/tmp/.X11-unix:rw
            - ./floorplan.yaml:/map.yaml:ro
            - ./floorplan.png:/map.png:ro
            - ./amcl.launch:/amcl_map.launch:ro
            - ./floorplan.world:/map.world:ro
            - ./local_robot_sim.launch:/local_robot_sim.launch:ro
        environment:
            - FIWAREHOST=orion
            - HOST=splocal
            - DISPLAY=$DISPLAY
            - SIMULATION=true
        ports: 
            - "39003:39003"

One rviz window is from the Local SP, where you can see the AGV's pose (red arrow) and the local updates (red tiny squares). Another rviz window is from the Central SP, where you can see the updates of the topology and new obstacles presented with blue tiny squares showing only the current position of the new obstacle. Vertices at the position of new obstacles are removed from the topology (blue squares become red, and connections are removed). All new obstacles are processed as they are received so only new ones are sent. That is the reason why in the Local SP you can see a trail of the obstacle, while in the Central SP there is no trail but the topology is permanently changed.