[Spinal][PwmTest] workaround to send pwm value to each servo independependently

aerial_robot_nerve/motor_test/CMakeLists.txt

@@ -9,12 +9,13 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
   geometry_msgs
   takasako_sps
+  spinal
 )
 
 find_package(Boost REQUIRED COMPONENTS system)
 
 catkin_package(
-  CATKIN_DEPENDS roscpp std_msgs std_srvs geometry_msgs takasako_sps
+  CATKIN_DEPENDS roscpp std_msgs std_srvs geometry_msgs takasako_sps spinal
 )
 
 include_directories(

aerial_robot_nerve/motor_test/package.xml

@@ -14,11 +14,13 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>takasako_sps</build_depend>
+  <build_depend>spinal</build_depend>
 
   <run_depend>geometry_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>takasako_sps</run_depend>
+  <run_depend>spinal</run_depend>
 
 </package>

aerial_robot_nerve/motor_test/src/motor_test.cpp

@@ -7,6 +7,7 @@
 #include <std_msgs/Empty.h>
 #include <std_srvs/Empty.h>
 #include <takasako_sps/PowerInfo.h>
+#include <spinal/PwmTest.h>
 
 namespace Mode
 {
@@ -39,7 +40,7 @@ class MotorTest
     nhp_.param("power_info_sub_name", topic_name, std::string("power_info"));
     power_info_sub_ = nh_.subscribe(topic_name, 1, &MotorTest::powerInfoCallback, this, ros::TransportHints().tcpNoDelay());
     nhp_.param("motor_pwm_sub_name", topic_name, std::string("power_pwm"));
-    motor_pwm_pub_ = nh_.advertise<std_msgs::Float32>(topic_name,1);
+    motor_pwm_pub_ = nh_.advertise<spinal::PwmTest>(topic_name,1);
 
     start_cmd_sub_ =  nh_.subscribe("start_log_cmd", 1,  &MotorTest::startCallback, this, ros::TransportHints().tcpNoDelay());
     sps_on_pub_ = nh.advertise<std_msgs::Empty>("/power_on_cmd", 1);
@@ -100,8 +101,8 @@ class MotorTest
 
     pwm_value_ = min_pwm_value_;
 
-    std_msgs::Float32 cmd_msg;
-    cmd_msg.data = pwm_value_  / pwm_range_;
+    spinal::PwmTest cmd_msg;
+    cmd_msg.pwms.push_back(pwm_value_  / pwm_range_);
     motor_pwm_pub_.publish(cmd_msg);
     init_time_ = ros::Time::now();
     ROS_INFO("start pwm test");
@@ -170,8 +171,8 @@ class MotorTest
               {
                 if(once_flag_)
                   {
-                    std_msgs::Float32 cmd_msg;
-                    cmd_msg.data = stop_pwm_value_ / pwm_range_;
+                    spinal::PwmTest cmd_msg;
+                    cmd_msg.pwms.push_back(stop_pwm_value_  / pwm_range_);
                     motor_pwm_pub_.publish(cmd_msg);
                     once_flag_ = false;
                     ROS_WARN("STOP");
@@ -206,8 +207,8 @@ class MotorTest
         if(pwm_value_ > max_pwm_value_)
           {
             start_flag_ = false;
-            std_msgs::Float32 cmd_msg;
-            cmd_msg.data = stop_pwm_value_ / pwm_range_;
+            spinal::PwmTest cmd_msg;
+            cmd_msg.pwms.push_back(stop_pwm_value_  / pwm_range_);
             motor_pwm_pub_.publish(cmd_msg);
 
             ROS_WARN("finish pwm test");
@@ -220,8 +221,8 @@ class MotorTest
         if(once_flag_)
           {
             ROS_INFO("target_pwm: %d", pwm_value_);
-            std_msgs::Float32 cmd_msg;
-            cmd_msg.data = pwm_value_ / pwm_range_;
+            spinal::PwmTest cmd_msg;
+            cmd_msg.pwms.push_back(pwm_value_  / pwm_range_);
             motor_pwm_pub_.publish(cmd_msg);
           }
       }

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -38,6 +38,7 @@ add_message_files(
   MotorInfo.msg
   PwmInfo.msg
   Pwms.msg
+  PwmTest.msg
   UavInfo.msg
   DesireCoord.msg
   FlightConfigCmd.msg

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp

@@ -653,10 +653,42 @@ void AttitudeController::maxYawGainIndex()
     }
 }
 
-void AttitudeController::pwmTestCallback(const std_msgs::Float32& pwm_msg)
+void AttitudeController::pwmTestCallback(const spinal::PwmTest& pwm_msg)
 {
-  pwm_test_flag_ = true;
-  pwm_test_value_ = pwm_msg.data; //2000ms
+#ifndef SIMULATION  
+  if(pwm_msg.pwms_length && !pwm_test_flag_)
+    {
+      pwm_test_flag_ = true;
+      nh_->logwarn("Enter pwm test mode");
+    }
+  else if(!pwm_msg.pwms_length && pwm_test_flag_)
+    {
+      pwm_test_flag_ = false;
+      nh_->logwarn("Escape from pwm test mode");
+      return;
+    }
+
+  if(pwm_msg.motor_index_length)
+    {
+      /*Individual test mode*/
+      if(pwm_msg.motor_index_length != pwm_msg.pwms_length)
+        {
+          nh_->logerror("The number of index does not match the number of pwms.");
+          return;
+        }
+      for(int i = 0; i < pwm_msg.motor_index_length; i++){
+        int motor_index = pwm_msg.motor_index[i];
+        pwm_test_value_[motor_index] = pwm_msg.pwms[i];
+      }
+    }
+  else
+    {
+      /*Simultaneous test mode*/
+      for(int i = 0; i < MAX_MOTOR_NUMBER; i++){
+        pwm_test_value_[i] = pwm_msg.pwms[0];
+      }
+    }
+#endif  
 }
 
 void AttitudeController::setStartControlFlag(bool start_control_flag)
@@ -801,7 +833,7 @@ void AttitudeController::pwmConversion()
     {
       for(int i = 0; i < MAX_MOTOR_NUMBER; i++)
         {
-          target_pwm_[i] = pwm_test_value_;
+          target_pwm_[i] = pwm_test_value_[i];
         }
       return;
     }

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h

@@ -39,6 +39,7 @@
 #include <std_msgs/Float32MultiArray.h>
 #include <std_srvs/SetBool.h>
 #include <spinal/Pwms.h>
+#include <spinal/PwmTest.h>
 #include <spinal/FourAxisCommand.h>
 #include <spinal/RollPitchYawTerms.h>
 #include <spinal/PwmInfo.h>
@@ -133,7 +134,7 @@ class AttitudeController
   ros::Subscriber<spinal::FourAxisCommand, AttitudeController> four_axis_cmd_sub_;
   ros::Subscriber<spinal::PwmInfo, AttitudeController> pwm_info_sub_;
   ros::Subscriber<spinal::RollPitchYawTerms, AttitudeController> rpy_gain_sub_;
-  ros::Subscriber<std_msgs::Float32, AttitudeController> pwm_test_sub_;
+  ros::Subscriber<spinal::PwmTest, AttitudeController> pwm_test_sub_;
   ros::Subscriber<spinal::PMatrixPseudoInverseWithInertia, AttitudeController> p_matrix_pseudo_inverse_inertia_sub_;
   ros::Subscriber<spinal::TorqueAllocationMatrixInv, AttitudeController> torque_allocation_matrix_inv_sub_;
   ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, AttitudeController> att_control_srv_;
@@ -195,7 +196,7 @@ class AttitudeController
   uint32_t voltage_update_last_time_;
   uint32_t control_term_pub_last_time_, control_feedback_state_pub_last_time_;
   uint32_t pwm_pub_last_time_;
-  float pwm_test_value_; // PWM Test
+  float pwm_test_value_[MAX_MOTOR_NUMBER]; // PWM Test
 
   void fourAxisCommandCallback( const spinal::FourAxisCommand &cmd_msg);
   void pwmInfoCallback( const spinal::PwmInfo &info_msg);
@@ -204,7 +205,7 @@ class AttitudeController
   void torqueAllocationMatrixInvCallback(const spinal::TorqueAllocationMatrixInv& msg);
   void thrustGainMapping();
   void maxYawGainIndex();
-  void pwmTestCallback(const std_msgs::Float32& pwm_msg);
+  void pwmTestCallback(const spinal::PwmTest& pwm_msg);
   void pwmConversion(void);
   void pwmsControl(void);
 

aerial_robot_nerve/spinal/msg/PwmTest.msg

@@ -0,0 +1,2 @@
+uint8[] motor_index
+float32[] pwms