Patrol agents internal position log sometimes becomes invalid after stalling in Stage

[jward0]

I'm using a modified version of the simulator that logs the xy coordinates of all patrol agents every second. The positions are logged from the patrol agent with id 0, from the PatrolAgent.xPos and patrolAgent.yPos attributes. I have noticed that if two robots collide in an unrecoverable fashion (a fairly rare occurrence, but running 6 agents using some strategies for multiple hours causes this to occur quite often) and the robots enter a stall state in Stage, their position as reported sometimes keeps varying, even though they have stalled in simulation. This disconnect between Stage's representation of the patrol agents and the ROS representation means that if the robots believe they are still moving, as they are not interacting with Stage properly they do not interact with obstacles in Stage and sometimes report positions that are inside obstacles. I have no idea how fixable this is or if it is even worth trying to address, as I have only seen it typically occur when the simulator is running for several hours continuously, but I felt I should make you aware of what I've found.

[Morphdust]

I've had this same issue, with the agents phasing through walls after a certain amount of time or after colliding with one another. I believe the cause is the conflict between the robot's noisy internal position estimate allowing them to reach places that patrolling_sim shouldn't allow them. The cleaner fix to this would be some extra code to monitor if an agent's true position is 'possible' (inside a wall or another agent or outside of the map).

In the meantime, as my application is not too concerned with realistic odometry models my solution has been to use the fake_localisation method of localising combined with modifying the model parameters that are passed to the Stage package.

In patrolling_sim/maps/ there is a file called crobot.inc which is called when each map is loaded and passed to Stage. In this file it defines the robot model as well as the localisation model parameters. You can either change the localisation method that is used to gps which provides an absolute true world position, or change your odom localisation to have [0 0 0 0] error, achieving the same goal but without having to change which one is called in your other code sections.

By doing this I've found that even when running the simulation for multiple hours and with high numbers of agents and speed-up multiples, I don't have any issues with robots drifting through walls or becoming lost in scenarios that they shouldn't be able to.

My thought of a solution is similar to what has been mentioned in stage_ros enhancement; some callback to execute this when an impossible position is achieved would be an avenue for anybody needing to include a realistic odometry model.