Skip to content

Commit

Permalink
Merge branch 'master' into pitch_hero_
Browse files Browse the repository at this point in the history 
# Conflicts:
#	include/rm_manual/manual_base.h
  • Loading branch information
@liyixin135
liyixin135 committed Jul 29, 2024
2 parents 9c4d6de + 89026b1 commit 1ec698e
Show file tree
Hide file tree
Showing 6 changed files with 54 additions and 33 deletions.
1 change: 1 addition & 0 deletions include/rm_manual/chassis_gimbal_manual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ class ChassisGimbalManual : public ManualBase
void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
void capacityDataCallback(const rm_msgs::PowerManagementSampleAndStatusData ::ConstPtr& data) override;
void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
void setChassisMode(int mode);
virtual void wPress()
{
x_scale_ = x_scale_ >= 1.0 ? 1.0 : x_scale_ + 1.0;
Expand Down
1 change: 1 addition & 0 deletions include/rm_manual/chassis_gimbal_shooter_manual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data) override;
void leftSwitchUpOn(ros::Duration duration);
void leftSwitchUpFall();
void mouseLeftPress();
Expand Down
6 changes: 5 additions & 1 deletion include/rm_manual/manual_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include <rm_msgs/GimbalPosState.h>
#include <rm_msgs/GameRobotStatus.h>
#include <rm_msgs/ManualToReferee.h>
#include <rm_msgs/ShootData.h>
#include <rm_msgs/PowerManagementSampleAndStatusData.h>
#include "rm_manual/input_event.h"

Expand Down Expand Up @@ -92,6 +93,9 @@ class ManualBase
virtual void suggestFireCallback(const std_msgs::Bool::ConstPtr& data)
{
}
virtual void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data)
{
}

// Referee
virtual void chassisOutputOn()
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_, gimbal_pos_state_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
20 changes: 20 additions & 0 deletions src/chassis_gimbal_manual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,4 +222,24 @@ void ChassisGimbalManual::mouseMidRise(double m_z)
}
}

void ChassisGimbalManual::setChassisMode(int mode)
{
switch (mode)
{
case rm_msgs::ChassisCmd::RAW:
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
is_gyro_ = true;
if (x_scale_ != 0.0 || y_scale_ != 0.0)
vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
else
vel_cmd_sender_->setAngularZVel(1.0);
break;
case rm_msgs::ChassisCmd::FOLLOW:
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
is_gyro_ = false;
vel_cmd_sender_->setAngularZVel(0.0);
break;
}
}

} // namespace rm_manual
47 changes: 21 additions & 26 deletions src/chassis_gimbal_shooter_manual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,13 @@ void ChassisGimbalShooterManual::suggestFireCallback(const std_msgs::Bool::Const
shooter_cmd_sender_->updateSuggestFireData(*data);
}

void ChassisGimbalShooterManual::shootDataCallback(const rm_msgs::ShootData::ConstPtr& data)
{
ChassisGimbalManual::shootDataCallback(data);
if (referee_is_online_ && !use_scope_)
shooter_cmd_sender_->updateShootData(*data);
}

void ChassisGimbalShooterManual::sendCommand(const ros::Time& time)
{
ChassisGimbalManual::sendCommand(time);
Expand Down Expand Up @@ -246,13 +253,11 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu
ChassisGimbalManual::updateRc(dbus_data);
if (std::abs(dbus_data->wheel) > 0.01)
{
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
is_gyro_ = true;
setChassisMode(rm_msgs::ChassisCmd::RAW);
}
else
{
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
is_gyro_ = false;
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
}
vel_cmd_sender_->setAngularZVel((std::abs(dbus_data->ch_r_y) > 0.01 || std::abs(dbus_data->ch_r_x) > 0.01) ?
dbus_data->wheel * gyro_rotate_reduction_ :
Expand Down Expand Up @@ -410,19 +415,12 @@ void ChassisGimbalShooterManual::cPress()
{
if (is_gyro_)
{
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
vel_cmd_sender_->setAngularZVel(0.0);
is_gyro_ = false;
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
}
else
{
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
is_gyro_ = true;
setChassisMode(rm_msgs::ChassisCmd::RAW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
if (x_scale_ != 0.0 || y_scale_ != 0.0)
vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
else
vel_cmd_sender_->setAngularZVel(1.0);
}
}

Expand Down Expand Up @@ -462,11 +460,11 @@ void ChassisGimbalShooterManual::wPress()
{
ChassisGimbalManual::wPress();
if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
gimbal_cmd_sender_->getEject())
(gimbal_cmd_sender_->getEject() && !use_scope_))
{
gimbal_cmd_sender_->setEject(false);
manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
}
}
Expand All @@ -475,11 +473,11 @@ void ChassisGimbalShooterManual::aPress()
{
ChassisGimbalManual::aPress();
if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
gimbal_cmd_sender_->getEject())
(gimbal_cmd_sender_->getEject() && !use_scope_))
{
gimbal_cmd_sender_->setEject(false);
manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
}
}
Expand All @@ -488,11 +486,11 @@ void ChassisGimbalShooterManual::sPress()
{
ChassisGimbalManual::sPress();
if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
gimbal_cmd_sender_->getEject())
(gimbal_cmd_sender_->getEject() && !use_scope_))
{
gimbal_cmd_sender_->setEject(false);
manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
}
}
Expand All @@ -501,11 +499,11 @@ void ChassisGimbalShooterManual::dPress()
{
ChassisGimbalManual::dPress();
if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
gimbal_cmd_sender_->getEject())
(gimbal_cmd_sender_->getEject() && !use_scope_))
{
gimbal_cmd_sender_->setEject(false);
manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
}
}
Expand Down Expand Up @@ -599,11 +597,7 @@ void ChassisGimbalShooterManual::vPress()
void ChassisGimbalShooterManual::shiftPress()
{
if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_)
{
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
vel_cmd_sender_->setAngularZVel(1.0);
is_gyro_ = false;
}
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
}

Expand Down Expand Up @@ -673,4 +667,5 @@ void ChassisGimbalShooterManual::ctrlQPress()
shooter_calibration_->reset();
gimbal_calibration_->reset();
}

} // namespace rm_manual
12 changes: 6 additions & 6 deletions src/manual_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,11 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
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,
&ManualBase::shootBeforehandCmdCallback, this);
suggest_fire_sub_ =
nh.subscribe<std_msgs::Bool>("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this);

game_robot_status_sub_ = nh_referee.subscribe<rm_msgs::GameRobotStatus>("game_robot_status", 10,
&ManualBase::gameRobotStatusCallback, this);
Expand All @@ -30,12 +35,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
"power_management/sample_and_status", 10, &ManualBase::capacityDataCallback, this);
power_heat_data_sub_ =
nh_referee.subscribe<rm_msgs::PowerHeatData>("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this);
suggest_fire_sub_ =
nh.subscribe<std_msgs::Bool>("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this);

shoot_beforehand_cmd_sub_ =
nh.subscribe<rm_msgs::ShootBeforehandCmd>("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10,
&ManualBase::shootBeforehandCmdCallback, this);
shoot_data_sub_ = nh_referee.subscribe<rm_msgs::ShootData>("shoot_data", 10, &ManualBase::shootDataCallback, this);

// pub
manual_to_referee_pub_ = nh.advertise<rm_msgs::ManualToReferee>("/manual_to_referee", 1);
Expand Down

0 comments on commit 1ec698e

Please sign in to comment.