[Robot Model] add a new flag to determine the model whether fixed or not

jsk-ros-pkg · Jun 28, 2023 · 8096f1f · 8096f1f
1 parent e174a93
commit 8096f1f
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 8 deletions.
diff --git a/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h
@@ -61,7 +61,7 @@ namespace aerial_robot_model {
   //Basic Aerial Robot Model
   class RobotModel {
   public:
-    RobotModel(bool init_with_rosparam = true, bool verbose = false, double fc_f_min_thre = 0, double fc_t_min_thre = 0, double epsilon = 10.0);
+    RobotModel(bool init_with_rosparam = true, bool verbose = false, bool fixed_model = true, double fc_f_min_thre = 0, double fc_t_min_thre = 0, double epsilon = 10.0);
     virtual ~RobotModel() = default;
 
     void updateRobotModel();
@@ -70,6 +70,7 @@ namespace aerial_robot_model {
 
     // kinematics
     const bool initialized() const { return initialized_; }
+    const bool isModelFixed() const {return fixed_model_; }
     const std::string getBaselinkName() const { return baselink_; }
     const std::map<std::string, KDL::RigidBodyInertia>& getInertiaMap() const { return inertia_map_; }
     const double getMass() const { return mass_; }
@@ -169,6 +170,7 @@ namespace aerial_robot_model {
 
     // kinematics
     bool initialized_;
+    bool fixed_model_;
     double mass_;
     urdf::Model model_;
     std::string baselink_;

diff --git a/aerial_robot_model/src/model/base_model/robot_model.cpp b/aerial_robot_model/src/model/base_model/robot_model.cpp
@@ -2,8 +2,9 @@
 
 namespace aerial_robot_model {
 
-  RobotModel::RobotModel(bool init_with_rosparam, bool verbose, double fc_f_min_thre, double fc_t_min_thre, double epsilon):
+  RobotModel::RobotModel(bool init_with_rosparam, bool verbose, bool fixed_model, double fc_f_min_thre, double fc_t_min_thre, double epsilon):
     verbose_(verbose),
+    fixed_model_(fixed_model),
     fc_f_min_thre_(fc_f_min_thre),
     fc_t_min_thre_(fc_t_min_thre),
     epsilon_(epsilon),
@@ -29,7 +30,7 @@ namespace aerial_robot_model {
     staticsInit();
 
     // update robot model instantly for fixed model
-    if (joint_num_ == 0) {
+    if (fixed_model_) {
       updateRobotModel();
     }
   }
@@ -276,7 +277,7 @@ namespace aerial_robot_model {
         extra_module_map_.insert(std::make_pair(module_name, extra_module));
         ROS_INFO("[extra module]: succeed to add new extra module %s", module_name.c_str());
 
-        if (joint_num_ == 0) {
+        if (fixed_model_) {
           // update robot model instantly
           updateRobotModel();
         }
@@ -303,7 +304,7 @@ namespace aerial_robot_model {
         extra_module_map_.erase(module_name);
         ROS_INFO("[extra module]: succeed to remove the extra module %s", module_name.c_str());
 
-        if (joint_num_ == 0) {
+        if (fixed_model_) {
           // update robot model instantly
           updateRobotModel();
         }

diff --git a/aerial_robot_model/src/model/base_model/robot_model_ros.cpp b/aerial_robot_model/src/model/base_model/robot_model_ros.cpp
@@ -28,7 +28,7 @@ namespace aerial_robot_model {
         robot_model_ = boost::make_shared<aerial_robot_model::RobotModel>();
       }
 
-    if (robot_model_->getJointNum() == 0) {
+    if (robot_model_->isModelFixed()) {
       // broadcast static tf between root and CoG
       // refering: https://github.com/ros/robot_state_publisher/blob/rolling/src/robot_state_publisher.cpp#L130
       geometry_msgs::TransformStamped tf = robot_model_->getCog<geometry_msgs::TransformStamped>();
@@ -50,7 +50,6 @@ namespace aerial_robot_model {
     joint_state_ = *state;
     robot_model_->updateRobotModel(*state);
 
-    // TODO1: tf for root -> cog in case of fixed aerial robot
     geometry_msgs::TransformStamped tf = robot_model_->getCog<geometry_msgs::TransformStamped>();
     tf.header = state->header;
     tf.header.frame_id = tf::resolve(tf_prefix_, robot_model_->getRootFrameName());

diff --git a/aerial_robot_model/src/model/transformable_model/robot_model.cpp b/aerial_robot_model/src/model/transformable_model/robot_model.cpp
@@ -3,7 +3,7 @@
 using namespace aerial_robot_model::transformable;
 
 RobotModel::RobotModel(bool init_with_rosparam, bool verbose, double fc_f_min_thre, double fc_t_min_thre, double epsilon):
-  aerial_robot_model::RobotModel(init_with_rosparam, verbose, fc_f_min_thre, fc_t_min_thre, epsilon)
+  aerial_robot_model::RobotModel(init_with_rosparam, verbose, false /* non-fixed model */, fc_f_min_thre, fc_t_min_thre, epsilon)
 {
   std::function<void (const KDL::TreeElement&) > recursiveSegmentCheck = [&recursiveSegmentCheck, this](const KDL::TreeElement& tree_element)
     {