Merge pull request #500 from tongtybj/PR/feature/visualize_rotor_speed

Enable visualizing the rotor speed in gazebo

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
     }
 
     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_;
 

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_));
     }

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());
           }
       }
   }

robots/hydrus/urdf/link.urdf.xacro

@@ -212,7 +212,7 @@
         </xacro:unless>
       </xacro:if>
       <xacro:unless value="${tilt_mode == 1}">
-        <origin rpy="0 0 0" xyz="${link_length* 0.5} 0 0"/>
+        <origin rpy="0 0 0" xyz="${link_length* 0.5} 0 0.013"/>
       </xacro:unless>
       <axis xyz="0 0 ${rotor_direction}"/>
     </joint>
@@ -233,6 +233,17 @@
           <cylinder length="0.1" radius="${protector_radius + 0.01}"/>
         </geometry>
       </collision>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0.032"/>
+        <geometry>
+          <xacro:if value="${rotor_direction == 1}">
+            <mesh filename="package://hydrus/urdf/mesh/propeller/T-motor-CF-14inch-CCW.dae"/>
+          </xacro:if>
+          <xacro:if value="${rotor_direction == -1}">
+            <mesh filename="package://hydrus/urdf/mesh/propeller/T-motor-CF-14inch-CW.dae"/>
+          </xacro:if>
+        </geometry>
+      </visual>
     </link>
     <xacro:damping_factor link ="thrust${self}"/>