[Ninja] Workaround to create new swarm formation mode ( = pseud assem…

…bly mode).
sugihara-16 · Jan 22, 2025 · 28935b0 · 28935b0
1 parent 13df758
commit 28935b0
Show file tree

Hide file tree

Showing 5 changed files with 203 additions and 143 deletions.
diff --git a/robots/beetle/include/beetle/control/beetle_controller.h b/robots/beetle/include/beetle/control/beetle_controller.h
@@ -42,6 +42,14 @@ namespace aerial_robot_control
     aerial_robot_msgs::PoseControlPid wrench_pid_msg_;
 
     map<string, ros::Subscriber> est_wrench_subs_;
+
+    void estExternalWrenchCallback(const beetle::TaggedWrench & msg);
+
+  protected:
+    std::map<int, Eigen::VectorXd> est_wrench_list_;
+    std::map<int, Eigen::VectorXd> inter_wrench_list_;
+    std::map<int, Eigen::VectorXd> wrench_comp_list_;
+    std::map<int, Eigen::VectorXd> ff_inter_wrench_list_;
 
     /* external wrench compensation */
     bool pd_wrench_comp_mode_;
@@ -62,15 +70,7 @@ namespace aerial_robot_control
     double I_comp_Fz_;
     double I_comp_Tx_;
     double I_comp_Ty_;
-    double I_comp_Tz_;
-
-    void estExternalWrenchCallback(const beetle::TaggedWrench & msg);
-
-  protected:
-    std::map<int, Eigen::VectorXd> est_wrench_list_;
-    std::map<int, Eigen::VectorXd> inter_wrench_list_;
-    std::map<int, Eigen::VectorXd> wrench_comp_list_;
-    std::map<int, Eigen::VectorXd> ff_inter_wrench_list_;
+    double I_comp_Tz_;    
 
     virtual void calcInteractionWrench();
     ros::Publisher tagged_external_wrench_pub_;

diff --git a/robots/ninja/include/ninja/control/ninja_controller.h b/robots/ninja/include/ninja/control/ninja_controller.h
@@ -31,6 +31,7 @@ namespace aerial_robot_control
                     double ctrl_loop_rate
                     ) override;
   private:
+    void pseudoAsmCallback(const std_msgs::BoolConstPtr & msg);
     boost::shared_ptr<aerial_robot_navigation::NinjaNavigator> ninja_navigator_;
     boost::shared_ptr<NinjaRobotModel> ninja_robot_model_;
 
@@ -42,6 +43,8 @@ namespace aerial_robot_control
 
     KDL::Tree module_tree_for_control_;
 
+    ros::Subscriber pseudo_assembly_flag_sub_;
+
   protected:
     void calcInteractionWrench() override;
     void externalWrenchEstimate() override;

diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -7,6 +7,7 @@
 #include <geometry_msgs/Pose.h>
 #include <regex>
 #include <aerial_robot_control/control/utils/pid.h>
+#include <std_msgs/Bool.h>
 
 namespace aerial_robot_navigation
 {
@@ -129,6 +130,8 @@ namespace aerial_robot_navigation
     inline tf::Vector3 getTargetVelCand() {return target_vel_candidate_;}
     inline tf::Vector3 getTargetOmegaCand() {return target_omega_candidate_;}
 
+    bool pseudo_assembly_mode_;
+
   protected:
     std::mutex mutex_com2base_;
 
@@ -199,6 +202,8 @@ namespace aerial_robot_navigation
 
     std::mutex mutex_cand_vel_;
     std::mutex mutex_cand_omega_;
+
+    double pseudo_cog_com_dist_;
 
   };
   template<> inline KDL::Frame NinjaNavigator::getCom2Base()

diff --git a/robots/ninja/src/control/ninja_controller.cpp b/robots/ninja/src/control/ninja_controller.cpp
@@ -21,6 +21,8 @@ namespace aerial_robot_control
     pid_controllers_.push_back(PID("joint_pitch", joint_p_gain_, joint_i_gain_, joint_d_gain_));
     pid_controllers_.push_back(PID("joint_yaw", joint_p_gain_, joint_i_gain_, joint_d_gain_));
     // ninja_robot_model_->copyTreeStructure(ninja_robot_model_->getInitModuleTree(), module_tree_for_control_);
+
+    pseudo_assembly_flag_sub_ = nh_.subscribe("/pseudo_assembly_flag",1,&NinjaController::pseudoAsmCallback, this);
   }
   bool NinjaController::update()
   {
@@ -246,6 +248,14 @@ namespace aerial_robot_control
         wrench_comp_list_[ninja_navigator_->getMyID()] = wrench_comp_cog;
       }
   }
+
+  void NinjaController::pseudoAsmCallback(const std_msgs::BoolConstPtr & msg)
+  {
+    ninja_navigator_->pseudo_assembly_mode_ = msg->data;
+    wrench_comp_p_gain_ = 0;
+    wrench_comp_d_gain_ = 0;
+    wrench_comp_i_gain_ = 0;
+  }
 
 
   void NinjaController::rosParamInit()