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

Raise pitch when image down #107

Closed
wants to merge 14 commits into from
Closed

Raise pitch when image down #107

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
ea01f84
Add ctrlRReleasing.
liyixin135 Jul 18, 2024
1c94185
Copy from x_event_.
liyixin135 Jul 18, 2024
cbad2df
Add a param and change rotate angle.
liyixin135 Jul 18, 2024
29879d1
Finish the tf.
liyixin135 Jul 18, 2024
1c36943
Finish the tf.
liyixin135 Jul 18, 2024
9f5b8f4
Update the condition of finishing pitch when image down.
liyixin135 Jul 18, 2024
60c3ee6
Add a flag for press ctrlR.
liyixin135 Jul 18, 2024
4511a06
Merge branch 'master' into pitch_hero_
liyixin135 Jul 18, 2024
d0ca57c
Add gimbalPosState sub.
liyixin135 Jul 18, 2024
64b79a6
Use pid state to replace tf.
liyixin135 Jul 18, 2024
5904f01
Modify wrong.
liyixin135 Jul 18, 2024
5452b74
Modify param name.
liyixin135 Jul 18, 2024
9c4d6de
Modify for clean.
liyixin135 Jul 20, 2024
1ec698e
Merge branch 'master' into pitch_hero_
liyixin135 Jul 29, 2024
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: 4 additions & 2 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 @@ -47,6 +47,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
void powerHeatDataCallback(const rm_msgs::PowerHeatData::ConstPtr& data) override;
void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
void gameStatusCallback(const rm_msgs::GameStatus::ConstPtr& data) override;
void gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data) override;
void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data) override;
void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
Expand Down Expand Up @@ -106,6 +107,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
void ctrlVPress();
void ctrlBPress();
void ctrlRPress();
void ctrlRReleasing();
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 +125,8 @@ 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,
bool prepare_shoot_ = false, turn_x_flag_ = false, turn_ctrl_r_flag_ = false, is_balance_ = false, use_scope_ = false,
adjust_image_transmission_ = false;
double yaw_current_{};
double yaw_current_{}, pitch_pos_error_{}, pitch_pos_des_{};
};
} // namespace rm_manual
6 changes: 5 additions & 1 deletion include/rm_manual/manual_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include <rm_msgs/ActuatorState.h>
#include <rm_msgs/ShootBeforehandCmd.h>
#include <rm_msgs/GimbalDesError.h>
#include <rm_msgs/GimbalPosState.h>
#include <rm_msgs/GameRobotStatus.h>
#include <rm_msgs/ManualToReferee.h>
#include <rm_msgs/ShootData.h>
Expand Down Expand Up @@ -73,6 +74,9 @@ class ManualBase
virtual void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data)
{
}
virtual void gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data)
{
}
virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data)
{
}
Expand Down Expand Up @@ -145,7 +149,7 @@ class ManualBase

ros::Subscriber odom_sub_, dbus_sub_, track_sub_, referee_sub_, capacity_sub_, game_status_sub_, joint_state_sub_,
game_robot_hp_sub_, actuator_state_sub_, power_heat_data_sub_, gimbal_des_error_sub_, game_robot_status_sub_,
suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_;
suggest_fire_sub_, shoot_beforehand_cmd_sub_, gimbal_pos_state_sub_, shoot_data_sub_;

sensor_msgs::JointState joint_state_;
rm_msgs::TrackData track_data_;
Expand Down
51 changes: 46 additions & 5 deletions src/chassis_gimbal_shooter_manual.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
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_.setActiveLow(boost::bind(&ChassisGimbalShooterManual::ctrlRReleasing, 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 @@ -140,6 +141,13 @@ void ChassisGimbalShooterManual::gimbalDesErrorCallback(const rm_msgs::GimbalDes
shooter_cmd_sender_->updateGimbalDesError(*data);
}

void ChassisGimbalShooterManual::gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data)
{
ManualBase::gimbalPosStateCallback(data);
pitch_pos_error_ = data->error;
pitch_pos_des_ = data->set_point;
}

void ChassisGimbalShooterManual::shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data)
{
ChassisGimbalManual::shootBeforehandCmdCallback(data);
Expand Down Expand Up @@ -196,7 +204,8 @@ void ChassisGimbalShooterManual::remoteControlTurnOff()
shooter_cmd_sender_->setZero();
shooter_calibration_->stop();
gimbal_calibration_->stop();
turn_flag_ = false;
turn_x_flag_ = false;
turn_ctrl_r_flag_ = false;
use_scope_ = false;
adjust_image_transmission_ = false;
}
Expand All @@ -214,7 +223,8 @@ void ChassisGimbalShooterManual::robotDie()
{
ManualBase::robotDie();
shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
turn_flag_ = false;
turn_x_flag_ = false;
turn_ctrl_r_flag_ = false;
use_scope_ = false;
adjust_image_transmission_ = false;
}
Expand Down Expand Up @@ -544,7 +554,7 @@ void ChassisGimbalShooterManual::dPressing()

void ChassisGimbalShooterManual::xPress()
{
turn_flag_ = true;
turn_x_flag_ = true;
geometry_msgs::PointStamped point_in;
try
{
Expand All @@ -565,7 +575,7 @@ void ChassisGimbalShooterManual::xPress()

void ChassisGimbalShooterManual::xReleasing()
{
if (turn_flag_)
if (turn_x_flag_)
{
gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
gimbal_cmd_sender_->setPoint(point_out_);
Expand All @@ -574,7 +584,7 @@ void ChassisGimbalShooterManual::xReleasing()
if (std::abs(angles::shortest_angular_distance(yaw, yaw_current_)) > finish_turning_threshold_)
{
gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
turn_flag_ = false;
turn_x_flag_ = false;
}
}
}
Expand Down Expand Up @@ -613,6 +623,37 @@ void ChassisGimbalShooterManual::ctrlRPress()
{
if (image_transmission_cmd_sender_)
adjust_image_transmission_ = !image_transmission_cmd_sender_->getState();
if (adjust_image_transmission_)
{
turn_ctrl_r_flag_ = true;
geometry_msgs::PointStamped point_in;
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 + 0.8;
tf2::doTransform(point_in, point_out_, tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)));
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
}
}
}

void ChassisGimbalShooterManual::ctrlRReleasing()
{
if (turn_ctrl_r_flag_)
{
gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
gimbal_cmd_sender_->setPoint(point_out_);
if (pitch_pos_error_ > -0.005 && pitch_pos_des_ < -0.6)
{
gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
turn_ctrl_r_flag_ = false;
}
}
}

void ChassisGimbalShooterManual::ctrlBPress()
Expand Down
2 changes: 2 additions & 0 deletions src/manual_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
track_sub_ = nh.subscribe<rm_msgs::TrackData>("/track", 10, &ManualBase::trackCallback, this);
gimbal_des_error_sub_ = nh.subscribe<rm_msgs::GimbalDesError>("/controllers/gimbal_controller/error", 10,
&ManualBase::gimbalDesErrorCallback, this);
gimbal_pos_state_sub_ = nh.subscribe<rm_msgs::GimbalPosState>("/controllers/gimbal_controller/pitch/pos_state", 10,
&ManualBase::gimbalPosStateCallback, this);
odom_sub_ = nh.subscribe<nav_msgs::Odometry>("/odom", 10, &ManualBase::odomCallback, this);
shoot_beforehand_cmd_sub_ =
nh.subscribe<rm_msgs::ShootBeforehandCmd>("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10,
Expand Down
Loading