Merge pull request #176 from liyixin135/master

Fix wrong

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -12,8 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
-gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.0, 0.105, 0.5)
-gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.0, 0.105, 0.5)
+gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.105, 0.0, 0.5)
+gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5)
 gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
 gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
 gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)

rm_gimbal_controllers/src/gimbal_base.cpp

@@ -301,6 +301,10 @@ void Controller::track(const ros::Time& time)
     ROS_WARN("%s", ex.what());
   }
   double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec());
+  while (yaw > M_PI)
+    yaw -= 2 * M_PI;
+  while (yaw < -M_PI)
+    yaw += 2 * M_PI;
   target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x;
   target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y;
   target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z;