Merge pull request rm-controls#141 from ye-luo-xi-tui/master

Avoid nan vel

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_{},
-      velocity_coeff_{}, power_offset_{};
+  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;

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);
 
@@ -281,10 +280,16 @@ void ChassisBase<T...>::updateOdom(const ros::Time& time, const ros::Duration& p
     // integral vel to pos and angle
     tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_);
     tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_);
-    odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec();
-    odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec();
-    odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec();
     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 < max_odom_vel_)
+    {
+      // avoid nan vel
+      odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec();
+      odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec();
+      odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec();
+    }
+    length =
         std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2));
     if (length > 0.001)
     {  // avoid nan quat

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -116,10 +116,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   double r = r1;
   double z = pos.z;
   track_target_ = std::abs(v_yaw) < max_track_target_vel_;
-  double switch_armor_angle =
-      track_target_ ?
-          acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ :
-          M_PI / 12;
+  double switch_armor_angle = track_target_ ?
+                                  acos(r / target_rho) - M_PI / 12 +
+                                      (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ :
+                                  M_PI / 12;
   if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
       (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.))
   {
@@ -287,13 +287,16 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
   else
   {
     geometry_msgs::Point target_pos_after_fly_time_and_delay{};
-    target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) -
-                   r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
-    target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) -
-                   r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
+    target_pos_after_fly_time_and_delay.x =
+        pos.x + vel.x * (fly_time_ + delay) -
+        r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
+    target_pos_after_fly_time_and_delay.y =
+        pos.y + vel.y * (fly_time_ + delay) -
+        r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
     target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay);
-    error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) +
-                      std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2));
+    error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) +
+                      std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) +
+                      std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2));
   }
   return error;
 }