Merge pull request #113 from liyixin135/new_pitch_hero

Update the method of rotate yaw and raise pitch
rm-controls · Jul 29, 2024 · 40b7a91 · 40b7a91
2 parents 89026b1 + 8a2b675
commit 40b7a91
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 31 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -88,7 +88,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   virtual void bPress();
   virtual void bRelease();
   virtual void rPress();
-  virtual void xReleasing();
+  virtual void xRelease();
   virtual void shiftPress();
   virtual void shiftRelease();
   void qPress()
@@ -106,6 +106,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void ctrlVPress();
   void ctrlBPress();
   void ctrlRPress();
+  void ctrlRRelease();
   virtual void ctrlQPress();
 
   InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_,
@@ -123,8 +124,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   geometry_msgs::PointStamped point_out_;
   uint8_t last_shoot_freq_{};
 
-  bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false,
-       adjust_image_transmission_ = false;
+  bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false;
   double yaw_current_{};
 };
 }  // namespace rm_manual
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -49,16 +49,17 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
                    boost::bind(&ChassisGimbalShooterManual::qRelease, this));
   b_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::bPress, this),
                    boost::bind(&ChassisGimbalShooterManual::bRelease, this));
-  x_event_.setRising(boost::bind(&ChassisGimbalShooterManual::xPress, this));
-  x_event_.setActiveLow(boost::bind(&ChassisGimbalShooterManual::xReleasing, this));
+  x_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::xPress, this),
+                   boost::bind(&ChassisGimbalShooterManual::xRelease, this));
   r_event_.setRising(boost::bind(&ChassisGimbalShooterManual::rPress, this));
   g_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gPress, this));
   v_event_.setRising(boost::bind(&ChassisGimbalShooterManual::vPress, this));
   ctrl_f_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlFPress, this));
   ctrl_v_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlVPress, this));
   ctrl_b_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlBPress, this));
   ctrl_q_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlQPress, this));
-  ctrl_r_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlRPress, this));
+  ctrl_r_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::ctrlRPress, this),
+                        boost::bind(&ChassisGimbalShooterManual::ctrlRRelease, this));
   shift_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::shiftPress, this),
                        boost::bind(&ChassisGimbalShooterManual::shiftRelease, this));
   mouse_left_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::mouseLeftPress, this));
@@ -196,7 +197,6 @@ void ChassisGimbalShooterManual::remoteControlTurnOff()
   shooter_cmd_sender_->setZero();
   shooter_calibration_->stop();
   gimbal_calibration_->stop();
-  turn_flag_ = false;
   use_scope_ = false;
   adjust_image_transmission_ = false;
 }
@@ -214,7 +214,6 @@ void ChassisGimbalShooterManual::robotDie()
 {
   ManualBase::robotDie();
   shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
-  turn_flag_ = false;
   use_scope_ = false;
   adjust_image_transmission_ = false;
 }
@@ -544,39 +543,22 @@ void ChassisGimbalShooterManual::dPressing()
 
 void ChassisGimbalShooterManual::xPress()
 {
-  turn_flag_ = true;
-  geometry_msgs::PointStamped point_in;
+  double roll{}, pitch{}, yaw{};
   try
   {
-    point_in.header.frame_id = "yaw";
-    point_in.point.x = -1.;
-    point_in.point.y = 0.;
-    point_in.point.z = tf_buffer_.lookupTransform("yaw", "pitch", ros::Time(0)).transform.translation.z;
-    tf2::doTransform(point_in, point_out_, tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)));
-
-    double roll{}, pitch{};
-    quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw_current_);
+    quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
   }
   catch (tf2::TransformException& ex)
   {
     ROS_WARN("%s", ex.what());
   }
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
+  gimbal_cmd_sender_->setGimbalTraj(yaw + M_PI, pitch);
 }
 
-void ChassisGimbalShooterManual::xReleasing()
+void ChassisGimbalShooterManual::xRelease()
 {
-  if (turn_flag_)
-  {
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
-    gimbal_cmd_sender_->setPoint(point_out_);
-    double roll{}, pitch{}, yaw{};
-    quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
-    if (std::abs(angles::shortest_angular_distance(yaw, yaw_current_)) > finish_turning_threshold_)
-    {
-      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
-      turn_flag_ = false;
-    }
-  }
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
 }
 
 void ChassisGimbalShooterManual::vPress()
@@ -613,6 +595,25 @@ void ChassisGimbalShooterManual::ctrlRPress()
 {
   if (image_transmission_cmd_sender_)
     adjust_image_transmission_ = !image_transmission_cmd_sender_->getState();
+  if (adjust_image_transmission_)
+  {
+    double roll{}, pitch{}, yaw{};
+    try
+    {
+      quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
+    }
+    catch (tf2::TransformException& ex)
+    {
+      ROS_WARN("%s", ex.what());
+    }
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
+    gimbal_cmd_sender_->setGimbalTraj(yaw, pitch - 0.64);
+  }
+}
+
+void ChassisGimbalShooterManual::ctrlRRelease()
+{
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
 }
 
 void ChassisGimbalShooterManual::ctrlBPress()