Add max_odom_vel and delete useless params.

rm-controls · Aug 6, 2023 · d9c6fe5 · d9c6fe5
1 parent 4162105
commit d9c6fe5
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
@@ -146,8 +146,9 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   hardware_interface::EffortJointInterface* effort_joint_interface_{};
   std::vector<hardware_interface::JointHandle> joint_handles_{};
 
-  double wheel_base_{}, wheel_track_{}, wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{},
+  double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{},
       velocity_coeff_{}, power_offset_{};
+  double max_odom_vel_;
   bool enable_odom_tf_ = false;
   bool topic_update_ = false;
   bool publish_odom_tf_ = false;

diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp
@@ -61,9 +61,8 @@ bool ChassisBase<T...>::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan
     return false;
   }
   wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02);
-  wheel_track_ = getParam(controller_nh, "wheel_track", 0.410);
-  wheel_base_ = getParam(controller_nh, "wheel_base", 0.320);
   twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6);
+  max_odom_vel_ = getParam(controller_nh,"max_odom_vel",0);
   enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true);
   publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false);
 
@@ -282,7 +281,7 @@ void ChassisBase<T...>::updateOdom(const ros::Time& time, const ros::Duration& p
     tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_);
     tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_);
     double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2));
-    if(length < 2000)
+    if(length < max_odom_vel_)
     {
       // avoid nan vel
       odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec();