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

I am using the AR Drone tumsimulator 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 (maxstep_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 the speed of the cycles, that maybe one of my iterations were occasionally taking too long to compute, so I set up a timing log.

For the iteration that crashes, I have times such as these:

Choice (ms): 33
Move (ms): 18
Stop (ms): 11
Total time (ms): 69

Total time being Choice, Move, and Stop times, plus a few other short code bits such as saving variables etc.

I also logged the drones position during the start and end of these timings so it can be seen how much it moved during each stage of the machine:

Move: 0.257697 rad, approx 14.8 degrees once move state completes
Stop: 0.042246 rad, approx 2.5 degrees once stop state completes
Total: 0.320877 rad, approx 18.4 degrees once all code for this iteration completes

I know that in the simulation the drone will drift slightly even when not moving, so that would account for a small amount of noise in the movement, but I can't explain the large jump in the Move state.

Here are some examples of times from other actions that didn't trigger any of my logging errors:

Move (ms): 11
Move: 0.036392 rad, approx 2.1 degrees

Move (ms): 13
Move: 0.045335 rad, approx 2.6 degrees

Move (ms): 18
Move: 0.021944 rad, approx 1.3 degrees

Move (ms): 24
Move: 0.025188 rad, approx 1.4 degrees

As can be seen from the times and distances above, the drone performs a reasonable action. However, the large error action timing falls within these same timings. So, it seems that even though my program sends the move and stop messages to the Gazebo simulator at approximately the same time rates, sometimes it seems to perform a much larger movement.

I have not been able to track down what causes this issue. I was told on a previous question that increasing the simulator speed makes it less accurate. I'm not sure if this is a side effect of that or a different issue.

I was hoping someone might be able to give me some ideas on what I should look into to fix this. Thank you.