The goal of this project is to add a danger tapes coorinates into occupancy grid with ROS and without depth camera.
Software:
- ROS melodic
- Opencv
- Python
The map’s programs allow printing the central points of danger tape’s squares on the ROS Occupancy grid map. Few programs allow users to use the map built by the SLAM algorithm combined with the map build by the danger tape’s recognition program. The picture represents how does this package work:
Combination of slam and tape's recognition https://youtu.be/MPohthiMvw4/0.jpg
The map in the ROS is represented as a 1D array that contains three numbers:
- -1
- 0
- 100
The number ‘-1’ means that the area is not explored before.
The number ‘0’ means that area is clear and there is no obstacle.
The number ‘100’ means that the area is occupied and there is an obstacle.
The main problem of the map without any preparations is that it is too hard to track the world coordinates (like X and Y) in a 1D array. Moreover, the map’s size may vary asymmetrically depending on the robot’s path.
Before starting working with and changing the map's information during the run, we must modify the map parameters and make them big enough to keep the map’s borders unchanged to prevent the map’s extension. In our example, the map has the size 30x30 m (1472 pixels on X coordinate and square of 1472 pixels for Y with the resolution of 0.02m).
After we prepared parameters for the map, we must modify our occupancy grid data according to the robot’s position and location of the danger tape relative to the robot. To do this, we are using transformation and rotation matrixes from the robot’s odometry to the camera’s position. Then, we are finding the distance from our camera to the tape’s points, that are lying on the ground, and defining its world coordinates.
Once we defined its world coordinate, we must change the value to ‘100’ at some point in the OccupancyGrid.data[]. Since the Occupancy_grid message representing the map as a 1D array, we must find the right index, where the value has to be changed. The next pictures explain how to find the right index of [OccupancyGrid.data[]] for particular X and Y coordinate:
Where:
Cx- center of the map in X coordinate
Cy – center of the map in Y coordinate
X – X coordinate of a point in the world’s frame
Y – Y coordinate of a point in the world’s frame
R – resolution of the map
H – number of the point in the height
(I created this equation by myself, probably it works not so good, but its working for this project)
Before starting make sure that the ROS-subscribers in all the programs have the right ROS-topics.
You can also modify the parameters for color masks according to your lighting and colors by running:
rosrun line_recognition cam_test.py
To start the program you can with following commands:
rosrun line_recognition cam.py
roslaunch line_recognition map.launch
The map program subscribes for data which will be generated by cam.pyprogram.
You can start these programs (map and camera) on the different machines within one ros network. (For instance you can run image processing on the nvidia jetson but your map will be on other remote machine)
Here you can see an example of merging two cards:
Tape map
Gmapping's map
Combined map of gmapping and tape's recognition
To start script for the map's combination you should use following command:
rosrun line_recognition map_sum
TBC
Here you can find few examples for this project:
(click on the picture to start video)
- Map.launch
the map.launch file starts 2 nodes: tf_map.py and map.py. In turn, these two nodes start the TF node for the danger tape’s map to place it in the same position as the default map built by SLAM, and the main map program creates the danger tape’s map (topic: /new_map).
You can run this launch file by the following command:
roslaunch line_recognition map.launch
- map_saver.launch
The map_saver.launch file saves map from specific topic (/combined_map default: /map) directly to the map folder in line_recognition package.
You can run this launch file by the following command:
roslaunch line_recognition map_saver.launch
- map_comb.launch
The map_comb.launch file starts 2 nodes: map_combination to combine SLAM and danger tape’s map and tf_map_sum.py to place the map in the right relative to the other maps.
You can run this launch file by the following command:
roslaunch line_recognition map_comb.launch
- set_arm_position.launch
The set_arm_position.launch file executes move_group.launch (arm’s movement planner) for Youbot arm generated by Moveit setup assistant. Then, the set_arm_position.launch file starts the set_exp_pos.py program, which sends a specific arm’s pose to the move_group node.
You can run this launch file by the following command:
roslaunch line_recognition set_arm_position.launch