Enable visualizing the rotor speed in gazebo

jsk-ros-pkg · Mar 27, 2022 · a2f0824 · a2f0824
1 parent e96e7ee
commit a2f0824
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 0 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/Jsk_Lib/flight_control/flight_control.h b/aerial_robot_nerve/spinal/mcu_project/Jsk_Lib/flight_control/flight_control.h
@@ -57,6 +57,7 @@ class FlightControl
     integrate_flag_ = false;
   }
 
+  inline bool getStartControlFlag() { return start_control_flag_; }
   inline AttitudeController& getAttController(){ return att_controller_;}
 
   void useGroundTruth(bool flag) { att_controller_.useGroundTruth(flag); }

diff --git a/aerial_robot_simulation/include/aerial_robot_simulation/rotor_handle.h b/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); // N/(rad/s) , this is a virtual linear rate of speed-f
     }
 
     inline std::string getName() const {return name_;}
@@ -102,13 +103,19 @@ namespace hardware_interface
     }
     inline void setCommand(double command); //no implement here
 
+    inline double getSpeed() const
+    {
+      return *force_ / speed_rate_;
+    }
+
   private:
     std::string name_;
     boost::shared_ptr<double> force_;
     int direction_;
     double f_pwm_rate_;
     double f_pwm_offset_;
     double m_f_rate_;
+    double speed_rate_;
     double pwm_rate_;
     double max_pwm_;
 

diff --git a/aerial_robot_simulation/include/aerial_robot_simulation/spinal_interface.h b/aerial_robot_simulation/include/aerial_robot_simulation/spinal_interface.h
@@ -113,6 +113,8 @@ namespace rotor_limits_interface
      */
     void enforceLimits(const ros::Duration& /* period */)
     {
+      if(jh_.getForce() == 0) return;
+
       /* because of "inline double setForce(double force)    {*force_ = force;}", we can change the value with same address */
       jh_.setForce(internal::saturate(jh_.getForce(), min_force_, max_force_));
     }

diff --git a/aerial_robot_simulation/src/aerial_robot_hw_sim.cpp b/aerial_robot_simulation/src/aerial_robot_hw_sim.cpp
@@ -376,6 +376,7 @@ namespace gazebo_ros_control
             auto  torque = rotor.getTorque();
             parent_link->AddRelativeTorque(gazebo::math::Vector3(torque.x(), torque.y(), torque.z()));
 #endif
+            sim_rotors_.at(j)->SetVelocity(0, rotor.getSpeed());
           }
       }
   }