From a2f0824f2ff9d297f9a14e2f916dff2b09129b5b Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Mon, 28 Mar 2022 01:12:12 +0900 Subject: [PATCH] Enable visualizing the rotor speed in gazebo --- .../mcu_project/Jsk_Lib/flight_control/flight_control.h | 1 + .../include/aerial_robot_simulation/rotor_handle.h | 7 +++++++ .../include/aerial_robot_simulation/spinal_interface.h | 2 ++ aerial_robot_simulation/src/aerial_robot_hw_sim.cpp | 1 + 4 files changed, 11 insertions(+) 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 index bc07fc3a8..46b3c8966 100644 --- 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 index 70df7299d..909d9280b 100644 --- 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,6 +103,11 @@ 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 force_; @@ -109,6 +115,7 @@ namespace hardware_interface 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 index 25c1a5928..fc14b78c3 100644 --- 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 index 58d0e0bd9..65eef6c9c 100644 --- 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()); } } }