Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update the method of rotate yaw and raise pitch #113

Merged
merged 1 commit into from
Jul 29, 2024
Merged

Update the method of rotate yaw and raise pitch #113

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions include/rm_manual/chassis_gimbal_shooter_manual.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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_,
Expand All @@ -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
57 changes: 29 additions & 28 deletions src/chassis_gimbal_shooter_manual.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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()
Expand Down
Loading