在Webot中的无人机运动之间造成延迟

问题描述

我正在使用Webots软件模拟无人机。我希望无人机起飞，并在达到所需的高度后，它将在该位置停留约5秒钟，然后自动启动其他动作。我尝试通过使用以下代码来创建该延迟：`

// Wait 5 seconds.
    while (wb_robot_step(timestep) != -1) {
      if (wb_robot_get_time() > 5.0)
        break;
    }`

但是它只有在控制环之外时才有效，当我在控制环中使用它时，它会影响无人机的性能，使无人机变得不平衡并疯狂地飞行。那么谁能告诉我我哪里出了问题，如果我想在无人机运动之间造成延迟，该怎么办？这是到目前为止我尝试过的动作代码：

// Actuate the motors taking into consideration all the computed inputs.
 double front_left_motor_input = k_vertical_thrust + vertical_input - roll_input - pitch_input + 
yaw_input;
 double front_right_motor_input = k_vertical_thrust + vertical_input + roll_input - pitch_input - 
yaw_input;
 double rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - 
yaw_input;
  double rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + 
yaw_input;
wb_motor_set_velocity(front_left_motor,front_left_motor_input);
wb_motor_set_velocity(front_right_motor,-front_right_motor_input);
wb_motor_set_velocity(rear_left_motor,-rear_left_motor_input);
wb_motor_set_velocity(rear_right_motor,rear_right_motor_input);

 // Wait 5 seconds.
while (wb_robot_step(timestep) != -1) {
  if (wb_robot_get_time() > 5.0)
    break;
}

  // Actuate the motors taking into consideration all the computed inputs. 2
 for (double i=-1.5;i<1;i+=0.5){
 if(i<0)
 roll_input = k_roll_p * CLAMP(roll,-1.0,1.0) + roll_acceleration +i ;
 else
 pitch_input = k_pitch_p * CLAMP(pitch,1.0) - pitch_acceleration +i ;
front_left_motor_input = k_vertical_thrust + vertical_input - roll_input -pitch_input  + yaw_input;
front_right_motor_input = k_vertical_thrust + vertical_input + roll_input-pitch_input  - yaw_input;
rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - yaw_input;
rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + yaw_input;
wb_motor_set_velocity(front_left_motor,rear_right_motor_input);}

解决方法

我不知道您的time_step变量的值，但是假设它是10，为什么不能简单地调用wb_robot_step(500 * time_step);？这样更简单。现在，关于无人机的行为，先前发送的命令将在等待期间继续应用，这可能是造成不稳定的原因。在进入等待时间之前，也许您应该考虑更改这些速度命令（将它们重置为0？）。