Robot seems to randomly make larger movements when Gazebo step size speeds up

edit

Sorry for the lengthy post, I tried to make it as short as possible.

I am using the AR Drone tum_simulator package in my project. I am trying to speed up the simulation in order to do faster testing. I realize there is an upper limit to the speedup I can get, but I'm unsure what that is. I am currently testing at a 20x speed up (max_step_size 0.02). I want to go faster, but I am running into a problem.

The following is a basic explanation of my program. It gives the drone certain actions to perform such as move forward, move up, move down, turn left, or turn right. To perform the actions, I'm using a finite state machine format that makes use of the ROS Rate sleep function. Basically, the finite state machine is in a loop with the sleep function. In the first Choice state, an action is chosen. Next it moves to the Move state where it sets a movement velocity the first time it enters this state. Then it stays in this move state for several cycles not issuing any commands in order to let the drone in the simulator move. After several movement cycles, we transition to the Stop state where the drones velocities are set to zero and given a couple cycles to lose its momentum and come to a stop. After this, we begin again at the choice state.

The code I am using is longer and more complicated, but the following is a rough example. Everything is working in my actual program, so if I made some mistake below it should just be an oversight when I cleaned the code up for posting.

EXECUTION_STATE state = CHOICE_STATE;
int move_count = 0;
int stop_count = 0;
int max_count = 5;
while (ros::ok()) {
    switch (state) {
        case MOVE_STATE:
            if (move_count == 0) {
                //set drone velocities to corresponding action
                this->execute_action();
            }
            move_count++;
            if (move_count > max_count) {
                //after drone has moved the desired number
                //of cycles, transition to stop state
                move_count = 0;
                state = STOP_STATE;
            }
            break;
        case STOP_STATE:
            if (stop_count == 0) {
                //set drone to zero velocity
                this->stop();
            }
            stop_count++;
            if (stop_count > max_count) {
                //after drone has had a few cycles to slow
                //down, transition to choosing another action
                stop_count = 0;
                state = CHOICE_STATE;
            }
            break;
        case CHOICE_STATE:
            //select which action to perform
            this->state_choice();
            state = MOVE_STATE;
            break;
    }
    //call to ros::Rate sleep
    this->sleep();
}

I know basing the action lengths on the cycle time of the loop won't give identical action distances, but I randomly see an extremely large action that triggers some debug asserts that I have in my program.

For example, for my turn left I might normally see a minimum of approximately 1 degree, approximately 5 degree maximum, and an average of 3 degrees. However, there are seemingly random instances where it is 20 degrees or more. These instances don't occur often. They usually take several thousand complete loops through the finite state machine before it happens.

Initially, I thought since movement distance was based on ... (more)