[simulation] revert getSpeed() to the previous definition, since osci…

…llation occurs when I use the new getSpeed(). Now the getSpeed() and getRPM() has no relationship.

aerial_robot_simulation/include/aerial_robot_simulation/rotor_handle.h

@@ -77,6 +77,7 @@ namespace hardware_interface
 
       motor_nh.param("rotor_force_noise", rotor_force_noise_, 0.0); // N
       motor_nh.param("dual_rotor_moment_noise", dual_rotor_moment_noise_, 0.0);
+      motor_nh.param("speed_rate", speed_rate_, 1.0); // rad/s/N , this is a virtual linear rate of speed-f
       motor_nh.param("krpm_rate", krpm_rate_, 0.1); // (kRPM)^2/N , this is a virtual linear rate of speed-f
     }
 
@@ -110,7 +111,7 @@ namespace hardware_interface
 
     inline double getSpeed() const
     {
-      return getRPM() * 2 * M_PI / 60;
+      return *force_ * speed_rate_;
     }
 
   private:
@@ -120,6 +121,7 @@ namespace hardware_interface
     double f_pwm_rate_;
     double f_pwm_offset_;
     double m_f_rate_;
+    double speed_rate_;
     double krpm_rate_;
     double pwm_rate_;
     double max_pwm_;