Skip to content

Commit

Permalink
Merge branch 'rm-controls:master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@yi123yi
yi123yi authored May 10, 2024
2 parents 87c89ec + 780b5bc commit 48597e2
Show file tree
Hide file tree
Showing 8 changed files with 488 additions and 379 deletions.
1 change: 1 addition & 0 deletions include/rm_manual/chassis_gimbal_shooter_cover_manual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
double exit_buff_mode_duration_{};
rm_common::SwitchDetectionCaller* switch_buff_srv_{};
rm_common::SwitchDetectionCaller* switch_buff_type_srv_{};
rm_common::SwitchDetectionCaller* switch_exposure_srv_{};
rm_common::JointPositionBinaryCommandSender* cover_command_sender_{};
InputEvent ctrl_z_event_, ctrl_q_event_, x_event_, z_event_;
std::string supply_frame_;
Expand Down
7 changes: 4 additions & 3 deletions include/rm_manual/chassis_gimbal_shooter_manual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
void gameStatusCallback(const rm_msgs::GameStatus::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;
void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
void leftSwitchUpOn(ros::Duration duration);
Expand Down Expand Up @@ -96,7 +97,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
{
shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::LOW);
}
void fPress()
void ctrlFPress()
{
shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
}
Expand All @@ -105,8 +106,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
void ctrlRPress();
virtual void ctrlQPress();

InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, f_event_, b_event_,
x_event_, r_event_, v_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, shift_event_,
InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_,
r_event_, v_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, shift_event_,
ctrl_shift_b_event_, mouse_left_event_, mouse_right_event_;
rm_common::ShooterCommandSender* shooter_cmd_sender_{};
rm_common::CameraSwitchCommandSender* camera_switch_cmd_sender_{};
Expand Down
120 changes: 70 additions & 50 deletions include/rm_manual/engineer_manual.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
#include <std_msgs/Float64.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <rm_msgs/EngineerAction.h>
#include <rm_msgs/MultiDofCmd.h>
#include <rm_msgs/GpioData.h>
#include <rm_msgs/EngineerUi.h>

namespace rm_manual
Expand Down Expand Up @@ -54,96 +56,114 @@ class EngineerManual : public ChassisGimbalManual
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
void sendCommand(const ros::Time& time) override;
void updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data);
void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
void gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data);
void stoneNumCallback(const std_msgs::String ::ConstPtr& data);
void sendCommand(const ros::Time& time) override;
void runStepQueue(const std::string& step_queue_name);
void actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback);
void actionDoneCallback(const actionlib::SimpleClientGoalState& state, const rm_msgs::EngineerResultConstPtr& result);
void runStepQueue(const std::string& step_queue_name);
void judgePrefix();
void judgeRoot();

void initMode();
void enterServo();
void actionActiveCallback()
{
operating_mode_ = MIDDLEWARE;
}
void remoteControlTurnOff() override;
void chassisOutputOn() override;
void rightSwitchDownRise() override;
void rightSwitchMidRise() override;
void gimbalOutputOn() override;

void rightSwitchUpRise() override;
void rightSwitchMidRise() override;
void rightSwitchDownRise() override;
void leftSwitchUpRise() override;
void leftSwitchUpFall();
void leftSwitchDownRise() override;
void leftSwitchDownFall();
void ctrlQPress();
void ctrlWPress();
void ctrlEPress();
void ctrlAPress();
void ctrlSPress();
void ctrlZPress();
void ctrlXPress();
void ctrlBPress();
void ctrlCPress();
void ctrlDPress();
void ctrlVRelease();
void ctrlVPress();
void ctrlBPress();
void ctrlEPress();
void ctrlFPress();
void ctrlGPress();
void ctrlQPress();
void ctrlRPress();
void ctrlSPress();
void ctrlVPress();
void ctrlVRelease();
void ctrlWPress();
void ctrlXPress();
void ctrlZPress();

void bPressing();
void bRelease();
void cPressing();
void cRelease();
void ePressing();
void eRelease();
void fPress();
void fRelease();
void gPress();
void gRelease();
void qPressing();
void qRelease();
void rPress();
void vPressing();
void vRelease();
void xPress();
void zPressing();
void zRelease();

void shiftPressing();
void shiftRelease();
void shiftZPress();
void shiftXPress();
void shiftCPress();
void shiftBPress();
void shiftBRelease();
void shiftCPress();
void shiftFPress();
void shiftGPress();
void shiftRPress();
void shiftRRelease();
void shiftVPress();
void shiftVRelease();
void rPress();
void qPressing();
void qRelease();
void ePressing();
void eRelease();
void zPressing();
void zRelease();
void xPress();
void cPressing();
void cRelease();
void bPressing();
void bRelease();
void vPressing();
void vRelease();
void fPressing();
void fRelease();
void gPressing();
void gRelease();
void shiftXPress();
void shiftZPress();

void mouseLeftRelease();
void mouseRightRelease();

int state_;
rm_msgs::EngineerUi engineer_ui_;
// Servo

bool change_flag_{};
double angular_z_scale_{}, gyro_scale_{}, fast_gyro_scale_{}, low_gyro_scale_{}, normal_gyro_scale_{},
exchange_gyro_scale_{}, fast_speed_scale_{}, low_speed_scale_{}, normal_speed_scale_{}, exchange_speed_scale_{};
;
std::string prefix_, root_, reversal_state_;
int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{}, gripper_state_{}, drag_state_{};
std::map<std::string, int> prefix_list_, root_list_;

std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" };
int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }, gimbal_height_{ 0 };

ros::Time last_time_;
ros::Subscriber reversal_vision_sub_;
ros::Subscriber stone_num_sub_, gripper_state_sub_;
ros::Publisher engineer_ui_pub_;

actionlib::SimpleActionClient<rm_msgs::EngineerAction> action_client_;
rm_common::CalibrationQueue *power_on_calibration_{}, *arm_calibration_{};
rm_msgs::GpioData gpio_state_;
rm_msgs::EngineerUi engineer_ui_;

rm_common::Vel3DCommandSender* servo_command_sender_;
rm_common::ServiceCallerBase<std_srvs::Empty>* servo_reset_caller_;
InputEvent left_switch_up_event_, left_switch_down_event_, ctrl_q_event_, ctrl_a_event_, ctrl_z_event_, ctrl_w_event_,
ctrl_s_event_, ctrl_x_event_, ctrl_e_event_, ctrl_d_event_, ctrl_c_event_, ctrl_b_event_, ctrl_v_event_, z_event_,
q_event_, e_event_, x_event_, c_event_, v_event_, b_event_, f_event_, shift_z_event_, shift_x_event_,
shift_c_event_, shift_v_event_, shift_b_event_, shift_g_event_, ctrl_r_event_, shift_q_event_, shift_e_event_,
ctrl_g_event_, shift_r_event_, ctrl_f_event_, shift_event_, g_event_, r_event_, mouse_left_event_,
rm_common::JointPositionBinaryCommandSender *extend_arm_a_command_sender_, *extend_arm_b_command_sender_;
rm_common::JointPointCommandSender *ore_bin_lifter_command_sender_, *ore_bin_rotate_command_sender_,
*gimbal_lifter_command_sender_;
rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_;

actionlib::SimpleActionClient<rm_msgs::EngineerAction> action_client_;

InputEvent left_switch_up_event_, left_switch_down_event_, ctrl_a_event_, ctrl_b_event_, ctrl_c_event_, ctrl_d_event_,
ctrl_e_event_, ctrl_f_event_, ctrl_g_event_, ctrl_q_event_, ctrl_r_event_, ctrl_s_event_, ctrl_v_event_,
ctrl_w_event_, ctrl_x_event_, ctrl_z_event_, b_event_, c_event_, e_event_, f_event_, g_event_, q_event_, r_event_,
v_event_, x_event_, z_event_, shift_event_, shift_b_event_, shift_c_event_, shift_e_event_, shift_f_event_,
shift_g_event_, shift_v_event_, shift_q_event_, shift_r_event_, shift_x_event_, shift_z_event_, mouse_left_event_,
mouse_right_event_;
};

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 @@ -30,6 +30,7 @@
#include <rm_msgs/BalanceState.h>
#include <rm_msgs/PowerHeatData.h>
#include <rm_msgs/ActuatorState.h>
#include <rm_msgs/ShootBeforehandCmd.h>
#include <rm_msgs/GimbalDesError.h>
#include <rm_msgs/GameRobotStatus.h>
#include <rm_msgs/ManualToReferee.h>
Expand Down Expand Up @@ -71,6 +72,9 @@ class ManualBase
virtual void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data)
{
}
virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data)
{
}
virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data)
{
}
Expand Down Expand Up @@ -137,7 +141,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_;
suggest_fire_sub_, shoot_beforehand_cmd_sub_;

sensor_msgs::JointState joint_state_;
rm_msgs::TrackData track_data_;
Expand Down
6 changes: 6 additions & 0 deletions src/chassis_gimbal_shooter_cover_manual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
switch_buff_srv_ = new rm_common::SwitchDetectionCaller(buff_switch_nh);
ros::NodeHandle buff_type_switch_nh(nh, "buff_type_switch");
switch_buff_type_srv_ = new rm_common::SwitchDetectionCaller(buff_type_switch_nh);
ros::NodeHandle exposure_switch_nh(nh, "exposure_switch");
switch_exposure_srv_ = new rm_common::SwitchDetectionCaller(exposure_switch_nh);
ros::NodeHandle chassis_nh(nh, "chassis");
normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1);
low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30);
Expand Down Expand Up @@ -150,9 +152,11 @@ void ChassisGimbalShooterCoverManual::ePress()
switch_buff_srv_->switchTargetType();
switch_detection_srv_->switchTargetType();
switch_buff_type_srv_->setTargetType(switch_buff_srv_->getTarget());
switch_exposure_srv_->switchTargetType();
switch_buff_srv_->callService();
switch_detection_srv_->callService();
switch_buff_type_srv_->callService();
switch_exposure_srv_->callService();
}

void ChassisGimbalShooterCoverManual::zPressing()
Expand Down Expand Up @@ -181,9 +185,11 @@ void ChassisGimbalShooterCoverManual::wPressing()
switch_buff_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR);
switch_detection_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR);
switch_buff_type_srv_->setTargetType(switch_buff_srv_->getTarget());
switch_exposure_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR);
switch_buff_srv_->callService();
switch_detection_srv_->callService();
switch_buff_type_srv_->callService();
switch_exposure_srv_->callService();
}
}

Expand Down
11 changes: 9 additions & 2 deletions src/chassis_gimbal_shooter_manual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,14 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
c_event_.setRising(boost::bind(&ChassisGimbalShooterManual::cPress, this));
q_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::qPress, this),
boost::bind(&ChassisGimbalShooterManual::qRelease, this));
f_event_.setRising(boost::bind(&ChassisGimbalShooterManual::fPress, 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));
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));
Expand Down Expand Up @@ -92,11 +92,11 @@ void ChassisGimbalShooterManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr
c_event_.update((!dbus_data->key_ctrl) & dbus_data->key_c);
g_event_.update(dbus_data->key_g & !dbus_data->key_ctrl);
q_event_.update((!dbus_data->key_ctrl) & dbus_data->key_q);
f_event_.update(dbus_data->key_f & !dbus_data->key_ctrl);
b_event_.update((!dbus_data->key_ctrl && !dbus_data->key_shift) & dbus_data->key_b);
x_event_.update(dbus_data->key_x & !dbus_data->key_ctrl);
r_event_.update((!dbus_data->key_ctrl) & dbus_data->key_r);
v_event_.update((!dbus_data->key_ctrl) & dbus_data->key_v);
ctrl_f_event_.update(dbus_data->key_f & dbus_data->key_ctrl);
ctrl_v_event_.update(dbus_data->key_ctrl & dbus_data->key_v);
ctrl_b_event_.update(dbus_data->key_ctrl & dbus_data->key_b & !dbus_data->key_shift);
ctrl_q_event_.update(dbus_data->key_ctrl & dbus_data->key_q);
Expand Down Expand Up @@ -139,10 +139,17 @@ void ChassisGimbalShooterManual::gimbalDesErrorCallback(const rm_msgs::GimbalDes
shooter_cmd_sender_->updateGimbalDesError(*data);
}

void ChassisGimbalShooterManual::shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data)
{
ChassisGimbalManual::shootBeforehandCmdCallback(data);
shooter_cmd_sender_->updateShootBeforehandCmd(*data);
}

void ChassisGimbalShooterManual::trackCallback(const rm_msgs::TrackData::ConstPtr& data)
{
ChassisGimbalManual::trackCallback(data);
shooter_cmd_sender_->updateTrackData(*data);
vel_cmd_sender_->updateTrackData(*data);
}

void ChassisGimbalShooterManual::suggestFireCallback(const std_msgs::Bool::ConstPtr& data)
Expand Down
Loading

0 comments on commit 48597e2

Please sign in to comment.