[Simulation] add ESC_Telemetry

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -44,6 +44,8 @@ add_message_files(
   FlightConfigCmd.msg
   Vector3Int16.msg
   TorqueAllocationMatrixInv.msg
+  ESCTelemetry.msg
+  ESCTelemetryArray.msg
   )
 
 add_service_files(

aerial_robot_nerve/spinal/msg/ESCTelemetry.msg

@@ -0,0 +1,8 @@
+# check the KISS protocol for details: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/
+
+int8 temperature  # 1 degree
+uint16 voltage  # 1000 = 10.00V
+uint16 current  # 1000 = 10.00A
+uint16 consumption  # 1mAh
+uint32 rpm  # 1 rpm
+uint8 crc_error  # calculated crc - received crc

aerial_robot_nerve/spinal/msg/ESCTelemetryArray.msg

@@ -0,0 +1,6 @@
+time stamp
+# spinal/ESCTelemetry[] esc_telemetry_array
+spinal/ESCTelemetry esc_telemetry_1
+spinal/ESCTelemetry esc_telemetry_2
+spinal/ESCTelemetry esc_telemetry_3
+spinal/ESCTelemetry esc_telemetry_4

aerial_robot_simulation/include/aerial_robot_simulation/aerial_robot_hw_sim.h

@@ -49,6 +49,7 @@
 #include <gazebo/sensors/MagnetometerSensor.hh>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include "spinal/ESCTelemetryArray.h"
 #include <kdl/tree.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 #include <nav_msgs/Odometry.h>
@@ -100,8 +101,10 @@ class AerialRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim
   ros::Subscriber sim_vel_sub_, sim_pos_sub_;
   ros::Publisher ground_truth_pub_;
   ros::Publisher mocap_pub_;
+  ros::Publisher esc_telem_pub_;
   double ground_truth_pub_rate_;
   double mocap_pub_rate_;
+  double esc_telem_pub_rate_;
 
   double mocap_rot_noise_, mocap_pos_noise_;
   double ground_truth_pos_noise_, ground_truth_vel_noise_, ground_truth_rot_noise_, ground_truth_angular_noise_;
@@ -112,7 +115,7 @@ class AerialRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim
   geometry_msgs::TwistStamped cmd_vel_;
   geometry_msgs::PoseStamped cmd_pos_;
 
-  ros::Time last_ground_truth_time_, last_mocap_time_;
+  ros::Time last_ground_truth_time_, last_mocap_time_, last_esc_telem_time_;
 
   void cmdVelCallback(const geometry_msgs::TwistStampedConstPtr& cmd_vel)
   {

aerial_robot_simulation/include/aerial_robot_simulation/rotor_handle.h

@@ -78,6 +78,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
     }
 
     inline std::string getName() const {return name_;}
@@ -103,6 +104,11 @@ namespace hardware_interface
     }
     inline void setCommand(double command); //no implement here
 
+    inline int getRPM() const
+    {
+      return (int)(sqrt(*force_ * krpm_rate_) * 1000);
+    }
+
     inline double getSpeed() const
     {
       return *force_ * speed_rate_;
@@ -116,6 +122,7 @@ namespace hardware_interface
     double f_pwm_offset_;
     double m_f_rate_;
     double speed_rate_;
+    double krpm_rate_;
     double pwm_rate_;
     double max_pwm_;
 

aerial_robot_simulation/src/aerial_robot_hw_sim.cpp

@@ -193,8 +193,10 @@ namespace gazebo_ros_control
     simulation_nh.param("mocap_pub_rate", mocap_pub_rate_, 0.01); // [sec]
     simulation_nh.param("mocap_pos_noise", mocap_pos_noise_, 0.001); // m
     simulation_nh.param("mocap_rot_noise", mocap_rot_noise_, 0.001); // rad
+    simulation_nh.param("esc_telem_pub_rate", esc_telem_pub_rate_, 0.02); // [sec]
     ground_truth_pub_ = model_nh.advertise<nav_msgs::Odometry>("ground_truth", 1);
     mocap_pub_ = model_nh.advertise<geometry_msgs::PoseStamped>("mocap/pose", 1);
+    esc_telem_pub_ = model_nh.advertise<spinal::ESCTelemetryArray>("esc_telem", 1);
 
     return true;
   }
@@ -310,6 +312,44 @@ namespace gazebo_ros_control
         mocap_pub_.publish(pose_msg);
         last_mocap_time_ = time;
       }
+
+      if (time.toSec() - last_esc_telem_time_.toSec() > esc_telem_pub_rate_)
+      {
+        spinal::ESCTelemetryArray esc_telem_msg;
+        esc_telem_msg.stamp = time;
+        if (rotor_n_dof_ > 4)
+        {
+          ROS_ERROR_STREAM("Sorry, currently the esc_telem_msg only support 4 to align with F55A ESC, so the ESC telemetry is not published");
+          return;
+        }
+
+        for (int j = 0; j < rotor_n_dof_; j++)
+        {
+          hardware_interface::RotorHandle rotor = spinal_interface_.getHandle(sim_rotors_.at(j)->GetName());
+          // rotor.getSpeed(); is rad/s. convert it to rpm
+          auto rpm = rotor.getRPM();
+
+          switch (j)
+          {
+            case 0:
+              esc_telem_msg.esc_telemetry_1.rpm = rpm;
+              break;
+            case 1:
+              esc_telem_msg.esc_telemetry_2.rpm = rpm;
+              break;
+            case 2:
+              esc_telem_msg.esc_telemetry_3.rpm = rpm;
+              break;
+            case 3:
+              esc_telem_msg.esc_telemetry_4.rpm = rpm;
+              break;
+            default:
+              break;
+          }
+        }
+        esc_telem_pub_.publish(esc_telem_msg);
+        last_esc_telem_time_ = time;
+      }
   }
 
   void AerialRobotHWSim::writeSim(ros::Time time, ros::Duration period)