Set traj mode for deploy and the traj scale

include/rm_manual/chassis_gimbal_manual.h

@@ -65,6 +65,7 @@ class ChassisGimbalManual : public ManualBase
   bool is_gyro_{ 0 };
   double speed_change_scale_{ 1. };
   double gimbal_scale_{ 1. };
+  double traj_scale_{ 0.5 };
   double gyro_move_reduction_{ 1. };
   double gyro_rotate_reduction_{ 1. };
   double finish_turning_threshold_{};

include/rm_manual/chassis_gimbal_shooter_manual.h

@@ -131,6 +131,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false,
        up_change_position_ = false, low_change_position_ = false, need_change_position_ = false;
   double yaw_current_{};
+  double traj_yaw_, traj_pitch_;
   double scale_;
 };
 }  // namespace rm_manual

src/chassis_gimbal_manual.cpp

@@ -21,6 +21,7 @@ ChassisGimbalManual::ChassisGimbalManual(ros::NodeHandle& nh, ros::NodeHandle& n
   ros::NodeHandle gimbal_nh(nh, "gimbal");
   gimbal_cmd_sender_ = new rm_common::GimbalCommandSender(gimbal_nh);
   gimbal_scale_ = getParam(gimbal_nh, "gimbal_scale", 1.0);
+  traj_scale_ = getParam(gimbal_nh, "traj_scale", 0.5);
   if (!gimbal_nh.getParam("finish_turning_threshold", finish_turning_threshold_))
     ROS_ERROR("Finish turning threshold no defined (namespace: %s)", nh.getNamespace().c_str());
 

src/chassis_gimbal_shooter_manual.cpp

@@ -311,6 +311,12 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu
     else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW)
       chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
   }
+  if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::TRAJ)
+  {
+    traj_yaw_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_yaw * ros::Duration(0.01).toSec();
+    traj_pitch_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_pitch * ros::Duration(0.01).toSec();
+    gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_);
+  }
 }
 
 void ChassisGimbalShooterManual::rightSwitchDownRise()
@@ -610,10 +616,26 @@ void ChassisGimbalShooterManual::zPress()
 {
   if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::RAW && !is_gyro_)
   {
+    double roll{}, pitch{}, yaw{};
+    try
+    {
+      quatToRPY(tf_buffer_.lookupTransform("base_link", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
+    }
+    catch (tf2::TransformException& ex)
+    {
+      ROS_WARN("%s", ex.what());
+    }
+    gimbal_cmd_sender_->setGimbalTrajFrameId("base_link");
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
+    traj_yaw_ = yaw, traj_pitch_ = -0.40;
+    gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_);
     setChassisMode(rm_msgs::ChassisCmd::DEPLOY);
   }
   else
+  {
     setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+  }
 }
 
 void ChassisGimbalShooterManual::shiftPress()
@@ -657,7 +679,7 @@ void ChassisGimbalShooterManual::ctrlRPress()
       ROS_WARN("%s", ex.what());
     }
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
-    gimbal_cmd_sender_->setGimbalTraj(yaw, pitch - 0.64);
+    gimbal_cmd_sender_->setGimbalTraj(yaw, -0.45);
   }
 }