Finally, I was able to solve main problems I was stuck with! One of them was that when robot was moving with a pallet on it, it was usually colliding with others as robot was not considering pallet’s dimensions on it. I proposed using dynamic_reconfigure package but adding it was to complex as most probably I should have been calling shell commands. I overcome it by tuning costmap params, mainly them:
inflation_radius: 1.5
cost_scaling_factor: 5.0
It helped to solve this problem, by forcing robot to move further from obstacles.
Additionally, I have stuck with problem that when robot was moving with a pallet on it, it was adding pallet’s legs into costmaps, and it was preventing robot’s movement at some points. I was simply using special rosservice clear_costmap and created API function that can be used in MyAlgorithm.
Both pick new pallet, movement with pallet and drop behaviors as well as clearing example can be seen in the following video:
Final solution example
Moreover, I have finished an implementation of the solution. Final structure of the solution is following:
- Wait for commands. If no commands received, go to charging location.
- If Goal is picked in GUI and Send Goal button pressed, go to the selected pallet and leave it in the drop region. Then return to the charging point.
- If Pick new Pallet button is pressed, go and pick it and leave it in the warehouse. Then, it too goes back to charging point.
- Waits for commands in the charging point.
Screencast of the solution can be accessed here:
If you want to replicate this exercise and solution itself please follow instructions in this link