Collision handling with misplaced robot in configuration file

[dfbarrero]

If a robot is initially placed on a wall, weird things happen.

[pedrosanchezr]

Adding some details and replication steps:
Example:

Config to replicate:

• Pathplanning scenario changing the size to 32 (no more changeS)

[pedrosanchezr]

I'm not being able to replicate again

[pedrosanchezr]

Replication details:

• Pathplanning
• Grid_size = 32
• Start = [18,13]

Details:
The position 18, 13 is valid, so we cannot solve it just checking if that position is free. It seems that for some reason the robot appears in 17, 13 pointing to 18, 13

[pedrosanchezr]

Notes:
Debugged Start position: X = 260, 200

[pedrosanchezr]

It seems that 260, 200 is always set at robot creation independently of the start position
If we debug utils.py create_robot, f['x'] and f['y'] are always 260 and 200

This value comes form the robot default configuration:

[pedrosanchezr]

If we change X to 270 in robot.conf, the and use the same configuration, the scenario works correctly.
So it seems that for some reason, there is a conflict on how we load the initial position and do the first movements.

[pedrosanchezr]

For some reason, the X and Y values before the "if gui" of display_image is 275, 197 but it changes at some point to 260, 200

[pedrosanchezr]

Notes in case I cannot fix it today:
It seems first execution of update(robots, npdata) changes it

[pedrosanchezr]

There is a weird behaviour in the update process.
Debugging it, if we check the value of X and Y before calling "self.__update_position(env, delta)" in robot.update(), it show us that the value before calling it is 275, 197 and after it's 260, 200

But if we check what is the value at the begining of self.__updateposition and at the end, it's:
from: 275, 197
to: 274.07, 197.0

So I'm thinking that this issue is related to some race condition and not related with the __update_position method itself

[pedrosanchezr]

Looking at it with fresh eyes I found I was debugging it wrong. The issue happens due to the next lines in robot.py:

if env.item(int(self.x), int(self.y)) is 50 or self.__crashing_radius(env):
self.x = self.last_pos[0]
self.y = self.last_pos[1]
self.collide((self.x, self.y))
else:
self.last_pos = (self.x, self.y)

It seems that we are using the position setup when the robot was instanciated as "last_pos" which is not calculated based on grid size or anything.

[pedrosanchezr]

Fixed adding a method to setup the last position.

As initial position on path_planning is setup after Robot instanciation (in pid_controller.py -> register_robot) there was a misalignment between position and last position. It kept as last_position whatever we had configured in robot.conf without taking into account path_planning configuration. This caused conflicts in some edge cases.

Fix is merged to develop and waiting to release to master.