[Beetle][Model] add accessor to leader id.

sugihara-16 · Dec 19, 2023 · 806192b · 806192b
1 parent 3c2cc92
commit 806192b
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/robots/beetle/include/beetle/model/beetle_robot_model.h b/robots/beetle/include/beetle/model/beetle_robot_model.h
@@ -45,6 +45,14 @@ class BeetleRobotModel : public GimbalrotorRobotModel{
     assembly_flags_[key] = value;
   }
   void setControlFlag(const bool control_flag){control_flag_ = control_flag;}
+  void setLeaderID(const int leader_id){
+    leader_id_ = leader_id;
+    leader_fix_flag_ = true;
+  }
+
+  void setLeaderFixFlag(const bool leader_fix_flag){
+    leader_fix_flag_ = leader_fix_flag;
+  }
 
   std::map<int, bool> getAssemblyFlags(){return assembly_flags_;}
   int getMaxModuleNum(){return max_modules_num_;}
@@ -71,6 +79,7 @@ class BeetleRobotModel : public GimbalrotorRobotModel{
   bool reconfig_flag_;
   bool current_assembled_;
   bool control_flag_;
+  bool leader_fix_flag_;
   std::vector<int> assembled_modules_ids_;
   int module_state_;
   int module_num_;

diff --git a/robots/beetle/src/model/beetle_robot_model.cpp b/robots/beetle/src/model/beetle_robot_model.cpp
@@ -5,7 +5,8 @@ BeetleRobotModel::BeetleRobotModel(bool init_with_rosparam, bool verbose, double
   tfBuffer_(),
   tfListener_(tfBuffer_),
   current_assembled_(false),
-  module_state_(SEPARATED)
+  module_state_(SEPARATED),
+  leader_fix_flag_(false)
 {
   for(int i = 0; i < max_modules_num_; i++){
     assembly_flags_.insert(std::make_pair(i+1,false));
@@ -74,7 +75,7 @@ void BeetleRobotModel::calcCenterOfMoving()
   //define a module closest to the center as leader
   std::sort(assembled_modules_ids_.begin(), assembled_modules_ids_.end());
   int leader_index = std::round((assembled_modules_ids_.size())/2.0) -1;
-  leader_id_ = assembled_modules_ids_[leader_index];
+  if(!leader_fix_flag_) leader_id_ = assembled_modules_ids_[leader_index];
   if(my_id_ == leader_id_ && control_flag_){
     module_state_ = LEADER;
   }else if(control_flag_){