From 71d4e7d421dc93d16f150e7230feff0f12b43234 Mon Sep 17 00:00:00 2001 From: cc0h <792166012@qq.com> Date: Mon, 11 Dec 2023 23:11:49 +0800 Subject: [PATCH 01/14] update manual. --- include/rm_manual/engineer_manual.h | 132 +++--- src/engineer_manual.cpp | 679 ++++++++++++++++------------ 2 files changed, 467 insertions(+), 344 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index f45da506..289ef312 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include namespace rm_manual @@ -54,97 +56,121 @@ 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_; - 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 prefix_list_, root_list_; + + // Servo + + + + + bool reversal_motion_{}, 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_{}, drag_state_{ "off" }, gripper_state_{"off"}; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{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 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::MultiDofCommandSender* reversal_command_sender_; rm_common::ServiceCallerBase* 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_, - mouse_right_event_; + rm_common::JointPositionBinaryCommandSender *drag_command_sender_; + rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_; + + actionlib::SimpleActionClient 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_; }; } // namespace rm_manual diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index a8813389..60a9f117 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -15,84 +15,103 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); + // Auto Exchange + XmlRpc::XmlRpcValue auto_exchange_value; + nh.getParam("auto_exchange", auto_exchange_value); + ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); +// auto_exchange_ = new auto_exchange::AutoExchange(auto_exchange_value, tf_buffer_, nh_auto_exchange); + // Pub +// exchanger_update_pub_ = nh.advertise("/is_update_exchanger", 1); + // Sub + stone_num_sub_ = nh.subscribe("/stone_num", 10, &EngineerManual::stoneNumCallback, this); + gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, + &EngineerManual::gpioStateCallback, this); + // Servo ros::NodeHandle nh_servo(nh, "servo"); servo_command_sender_ = new rm_common::Vel3DCommandSender(nh_servo); servo_reset_caller_ = new rm_common::ServiceCallerBase(nh_servo, "/servo_server/reset_servo_status"); // Vel ros::NodeHandle chassis_nh(nh, "chassis"); - if (!chassis_nh.getParam("fast_speed_scale", fast_speed_scale_)) - fast_speed_scale_ = 1; - if (!chassis_nh.getParam("normal_speed_scale", normal_speed_scale_)) - normal_speed_scale_ = 0.5; - if (!chassis_nh.getParam("low_speed_scale", low_speed_scale_)) - low_speed_scale_ = 0.30; - if (!chassis_nh.getParam("exchange_speed_scale", exchange_speed_scale_)) - exchange_speed_scale_ = 0.30; - if (!chassis_nh.getParam("fast_gyro_scale", fast_gyro_scale_)) - fast_gyro_scale_ = 0.5; - if (!chassis_nh.getParam("normal_gyro_scale", normal_gyro_scale_)) - normal_gyro_scale_ = 0.15; - if (!chassis_nh.getParam("low_gyro_scale", low_gyro_scale_)) - low_gyro_scale_ = 0.05; - if (!chassis_nh.getParam("exchange_gyro_scale", exchange_gyro_scale_)) - exchange_gyro_scale_ = 0.12; + chassis_nh.param("fast_speed_scale", fast_speed_scale_, 1.0); + chassis_nh.param("normal_speed_scale", normal_speed_scale_, 0.5); + chassis_nh.param("low_speed_scale", low_speed_scale_, 0.3); + chassis_nh.param("exchange_speed_scale", exchange_speed_scale_, 0.3); + chassis_nh.param("fast_gyro_scale", fast_gyro_scale_, 0.5); + chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); + chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); + chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); + ros::NodeHandle nh_reversal(nh, "reversal"); + reversal_command_sender_ = new rm_common::MultiDofCommandSender(nh_reversal); + // Calibration XmlRpc::XmlRpcValue rpc_value; - nh.getParam("power_on_calibration", rpc_value); - power_on_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); - nh.getParam("arm_calibration", rpc_value); - arm_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); + nh.getParam("pitch_calibration", rpc_value); + pitch_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); + nh.getParam("calibration_gather", rpc_value); + calibration_gather_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); + // Key binding left_switch_up_event_.setFalling(boost::bind(&EngineerManual::leftSwitchUpFall, this)); left_switch_up_event_.setRising(boost::bind(&EngineerManual::leftSwitchUpRise, this)); left_switch_down_event_.setFalling(boost::bind(&EngineerManual::leftSwitchDownFall, this)); - ctrl_q_event_.setRising(boost::bind(&EngineerManual::ctrlQPress, this)); + left_switch_down_event_.setRising(boost::bind(&EngineerManual::leftSwitchDownRise, this)); ctrl_a_event_.setRising(boost::bind(&EngineerManual::ctrlAPress, this)); - ctrl_z_event_.setRising(boost::bind(&EngineerManual::ctrlZPress, this)); - ctrl_w_event_.setRising(boost::bind(&EngineerManual::ctrlWPress, this)); - ctrl_s_event_.setRising(boost::bind(&EngineerManual::ctrlSPress, this)); - ctrl_x_event_.setRising(boost::bind(&EngineerManual::ctrlXPress, this)); - ctrl_e_event_.setRising(boost::bind(&EngineerManual::ctrlEPress, this)); - ctrl_d_event_.setRising(boost::bind(&EngineerManual::ctrlDPress, this)); - ctrl_c_event_.setRising(boost::bind(&EngineerManual::ctrlCPress, this)); - ctrl_v_event_.setRising(boost::bind(&EngineerManual::ctrlVPress, this)); - ctrl_v_event_.setFalling(boost::bind(&EngineerManual::ctrlVRelease, this)); ctrl_b_event_.setRising(boost::bind(&EngineerManual::ctrlBPress, this)); + ctrl_c_event_.setRising(boost::bind(&EngineerManual::ctrlCPress, this)); + ctrl_d_event_.setRising(boost::bind(&EngineerManual::ctrlDPress, this)); + ctrl_e_event_.setRising(boost::bind(&EngineerManual::ctrlEPress, this)); ctrl_f_event_.setRising(boost::bind(&EngineerManual::ctrlFPress, this)); ctrl_g_event_.setRising(boost::bind(&EngineerManual::ctrlGPress, this)); + ctrl_q_event_.setRising(boost::bind(&EngineerManual::ctrlQPress, this)); ctrl_r_event_.setRising(boost::bind(&EngineerManual::ctrlRPress, this)); + ctrl_s_event_.setRising(boost::bind(&EngineerManual::ctrlSPress, this)); + ctrl_v_event_.setRising(boost::bind(&EngineerManual::ctrlVPress, this)); + ctrl_v_event_.setFalling(boost::bind(&EngineerManual::ctrlVRelease, this)); + ctrl_w_event_.setRising(boost::bind(&EngineerManual::ctrlWPress, this)); + ctrl_x_event_.setRising(boost::bind(&EngineerManual::ctrlXPress, this)); + ctrl_z_event_.setRising(boost::bind(&EngineerManual::ctrlZPress, this)); + b_event_.setActiveHigh(boost::bind(&EngineerManual::bPressing, this)); + b_event_.setFalling(boost::bind(&EngineerManual::bRelease, this)); + c_event_.setActiveHigh(boost::bind(&EngineerManual::cPressing, this)); + c_event_.setFalling(boost::bind(&EngineerManual::cRelease, this)); e_event_.setActiveHigh(boost::bind(&EngineerManual::ePressing, this)); - q_event_.setActiveHigh(boost::bind(&EngineerManual::qPressing, this)); e_event_.setFalling(boost::bind(&EngineerManual::eRelease, this)); + f_event_.setRising(boost::bind(&EngineerManual::fPress, this)); + f_event_.setFalling(boost::bind(&EngineerManual::fRelease, this)); + g_event_.setRising(boost::bind(&EngineerManual::gPress, this)); + g_event_.setFalling(boost::bind(&EngineerManual::gRelease, this)); + q_event_.setActiveHigh(boost::bind(&EngineerManual::qPressing, this)); q_event_.setFalling(boost::bind(&EngineerManual::qRelease, this)); - z_event_.setActiveHigh(boost::bind(&EngineerManual::zPressing, this)); - z_event_.setFalling(boost::bind(&EngineerManual::zRelease, this)); - c_event_.setActiveHigh(boost::bind(&EngineerManual::cPressing, this)); - c_event_.setFalling(boost::bind(&EngineerManual::cRelease, this)); r_event_.setRising(boost::bind(&EngineerManual::rPress, this)); v_event_.setActiveHigh(boost::bind(&EngineerManual::vPressing, this)); v_event_.setFalling(boost::bind(&EngineerManual::vRelease, this)); - g_event_.setActiveHigh(boost::bind(&EngineerManual::gPressing, this)); - g_event_.setFalling(boost::bind(&EngineerManual::gRelease, this)); - b_event_.setActiveHigh(boost::bind(&EngineerManual::bPressing, this)); - b_event_.setFalling(boost::bind(&EngineerManual::bRelease, this)); - f_event_.setActiveHigh(boost::bind(&EngineerManual::fPressing, this)); - f_event_.setFalling(boost::bind(&EngineerManual::fRelease, this)); - shift_z_event_.setRising(boost::bind(&EngineerManual::shiftZPress, this)); - shift_c_event_.setRising(boost::bind(&EngineerManual::shiftCPress, this)); - shift_v_event_.setRising(boost::bind(&EngineerManual::shiftVPress, this)); - shift_v_event_.setFalling(boost::bind(&EngineerManual::shiftVRelease, this)); + x_event_.setRising(boost::bind(&EngineerManual::xPress,this)); + z_event_.setActiveHigh(boost::bind(&EngineerManual::zPressing, this)); + z_event_.setFalling(boost::bind(&EngineerManual::zRelease, this)); + shift_event_.setActiveHigh(boost::bind(&EngineerManual::shiftPressing, this)); + shift_event_.setFalling(boost::bind(&EngineerManual::shiftRelease, this)); shift_b_event_.setRising(boost::bind(&EngineerManual::shiftBPress, this)); shift_b_event_.setFalling(boost::bind(&EngineerManual::shiftBRelease, this)); - shift_x_event_.setRising(boost::bind(&EngineerManual::shiftXPress, this)); + shift_c_event_.setRising(boost::bind(&EngineerManual::shiftCPress, this)); + shift_f_event_.setRising(boost::bind(&EngineerManual::shiftFPress, this)); shift_g_event_.setRising(boost::bind(&EngineerManual::shiftGPress, this)); shift_r_event_.setRising(boost::bind(&EngineerManual::shiftRPress, this)); - shift_event_.setActiveHigh(boost::bind(&EngineerManual::shiftPressing, this)); - shift_event_.setFalling(boost::bind(&EngineerManual::shiftRelease, this)); + shift_r_event_.setFalling(boost::bind(&EngineerManual::shiftRRelease, this)); + shift_v_event_.setRising(boost::bind(&EngineerManual::shiftVPress, this)); + shift_v_event_.setFalling(boost::bind(&EngineerManual::shiftVRelease, this)); + shift_x_event_.setRising(boost::bind(&EngineerManual::shiftXPress, this)); + shift_z_event_.setRising(boost::bind(&EngineerManual::shiftZPress, this)); + mouse_left_event_.setFalling(boost::bind(&EngineerManual::mouseLeftRelease, this)); mouse_right_event_.setFalling(boost::bind(&EngineerManual::mouseRightRelease, this)); } +void EngineerManual::run() +{ + ChassisGimbalManual::run(); + calibration_gather_->update(ros::Time::now()); + engineer_ui_pub_.publish(engineer_ui_); +} void EngineerManual::changeSpeedMode(SpeedMode speed_mode) { if (speed_mode == LOW) @@ -116,55 +135,45 @@ void EngineerManual::changeSpeedMode(SpeedMode speed_mode) gyro_scale_ = exchange_gyro_scale_; } } -void EngineerManual::run() -{ - ChassisGimbalManual::run(); - power_on_calibration_->update(ros::Time::now(), state_ != PASSIVE); - arm_calibration_->update(ros::Time::now()); - engineer_ui_pub_.publish(engineer_ui_); -} - void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::checkKeyboard(dbus_data); - ctrl_q_event_.update(dbus_data->key_ctrl & dbus_data->key_q); ctrl_a_event_.update(dbus_data->key_ctrl & dbus_data->key_a); - ctrl_z_event_.update(dbus_data->key_ctrl & dbus_data->key_z); - ctrl_w_event_.update(dbus_data->key_ctrl & dbus_data->key_w); - ctrl_s_event_.update(dbus_data->key_ctrl & dbus_data->key_s); - ctrl_x_event_.update(dbus_data->key_ctrl & dbus_data->key_x); - ctrl_e_event_.update(dbus_data->key_ctrl & dbus_data->key_e); - ctrl_d_event_.update(dbus_data->key_ctrl & dbus_data->key_d); + ctrl_b_event_.update(dbus_data->key_ctrl & dbus_data->key_b); ctrl_c_event_.update(dbus_data->key_ctrl & dbus_data->key_c); - ctrl_v_event_.update(dbus_data->key_ctrl & dbus_data->key_v); + ctrl_d_event_.update(dbus_data->key_ctrl & dbus_data->key_d); ctrl_e_event_.update(dbus_data->key_ctrl & dbus_data->key_e); - ctrl_b_event_.update(dbus_data->key_ctrl & dbus_data->key_b); + ctrl_f_event_.update(dbus_data->key_ctrl & dbus_data->key_f); + ctrl_g_event_.update(dbus_data->key_ctrl & dbus_data->key_g); + ctrl_q_event_.update(dbus_data->key_ctrl & dbus_data->key_q); ctrl_r_event_.update(dbus_data->key_ctrl & dbus_data->key_r); - ctrl_g_event_.update(dbus_data->key_g & dbus_data->key_ctrl); - ctrl_f_event_.update(dbus_data->key_f & dbus_data->key_ctrl); + ctrl_s_event_.update(dbus_data->key_ctrl & dbus_data->key_s); + ctrl_v_event_.update(dbus_data->key_ctrl & dbus_data->key_v); + ctrl_w_event_.update(dbus_data->key_ctrl & dbus_data->key_w); + ctrl_x_event_.update(dbus_data->key_ctrl & dbus_data->key_x); + ctrl_z_event_.update(dbus_data->key_ctrl & dbus_data->key_z); - z_event_.update(dbus_data->key_z & !dbus_data->key_ctrl & !dbus_data->key_shift); - x_event_.update(dbus_data->key_x & !dbus_data->key_ctrl & !dbus_data->key_shift); - c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); - v_event_.update(dbus_data->key_v & !dbus_data->key_ctrl & !dbus_data->key_shift); b_event_.update(dbus_data->key_b & !dbus_data->key_ctrl & !dbus_data->key_shift); - g_event_.update(dbus_data->key_g & !dbus_data->key_ctrl & !dbus_data->key_shift); + c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); + e_event_.update(dbus_data->key_e & !dbus_data->key_ctrl & !dbus_data->key_shift); f_event_.update(dbus_data->key_f & !dbus_data->key_ctrl & !dbus_data->key_shift); + g_event_.update(dbus_data->key_g & !dbus_data->key_ctrl & !dbus_data->key_shift); + q_event_.update(dbus_data->key_q & !dbus_data->key_ctrl & !dbus_data->key_shift); r_event_.update(dbus_data->key_r & !dbus_data->key_ctrl & !dbus_data->key_shift); - q_event_.update(dbus_data->key_q & !dbus_data->key_ctrl); - e_event_.update(dbus_data->key_e & !dbus_data->key_ctrl); + v_event_.update(dbus_data->key_v & !dbus_data->key_ctrl & !dbus_data->key_shift); + x_event_.update(dbus_data->key_x & !dbus_data->key_ctrl & !dbus_data->key_shift); + z_event_.update(dbus_data->key_z & !dbus_data->key_ctrl & !dbus_data->key_shift); - shift_z_event_.update(dbus_data->key_shift & dbus_data->key_z); - shift_x_event_.update(dbus_data->key_shift & dbus_data->key_x); - shift_c_event_.update(dbus_data->key_shift & dbus_data->key_c); - shift_v_event_.update(dbus_data->key_shift & dbus_data->key_v); + shift_event_.update(dbus_data->key_shift & !dbus_data->key_ctrl); shift_b_event_.update(dbus_data->key_shift & dbus_data->key_b); - shift_q_event_.update(dbus_data->key_shift & dbus_data->key_q); + shift_c_event_.update(dbus_data->key_shift & dbus_data->key_c); shift_e_event_.update(dbus_data->key_shift & dbus_data->key_e); - shift_r_event_.update(dbus_data->key_shift & dbus_data->key_r); shift_g_event_.update(dbus_data->key_shift & dbus_data->key_g); + shift_q_event_.update(dbus_data->key_shift & dbus_data->key_q); + shift_r_event_.update(dbus_data->key_shift & dbus_data->key_r); + shift_v_event_.update(dbus_data->key_shift & dbus_data->key_v); shift_x_event_.update(dbus_data->key_shift & dbus_data->key_x); - shift_event_.update(dbus_data->key_shift & !dbus_data->key_ctrl); + shift_z_event_.update(dbus_data->key_shift & dbus_data->key_z); mouse_left_event_.update(dbus_data->p_l); mouse_right_event_.update(dbus_data->p_r); @@ -172,17 +181,11 @@ void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); } -void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) -{ - ManualBase::dbusDataCallback(data); - chassis_cmd_sender_->updateRefereeStatus(referee_is_online_); - updateServo(data); -} - void EngineerManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::updateRc(dbus_data); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + chassis_cmd_sender_->getMsg()->command_source_frame = "base_link"; vel_cmd_sender_->setAngularZVel(dbus_data->wheel); vel_cmd_sender_->setLinearXVel(dbus_data->ch_r_y); vel_cmd_sender_->setLinearYVel(-dbus_data->ch_r_x); @@ -190,12 +193,68 @@ void EngineerManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); left_switch_down_event_.update(dbus_data->s_l == rm_msgs::DbusData::DOWN); } - void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { - ChassisGimbalManual::updatePc(dbus_data); + checkKeyboard(dbus_data); left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + if (!reversal_motion_ && servo_mode_ == JOINT) + reversal_command_sender_->setGroupValue(0., 0., 5 * dbus_data->ch_r_y, 5 * dbus_data->ch_l_x, 5 * dbus_data->ch_l_y, + 0.); +} +void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ +// if (auto_exchange_->union_move_->getEnterFlag() != true && !auto_exchange_->getFinishFlag()) +// { + servo_command_sender_->setLinearVel(dbus_data->wheel, -dbus_data->ch_l_x, dbus_data->ch_l_y); + servo_command_sender_->setAngularVel(-angular_z_scale_, dbus_data->ch_r_y, dbus_data->ch_r_x); +// } +// else +// { +// if (!auto_exchange_->union_move_->getFinishFlag()) +// { +// std::vector servo_scales; +// servo_scales.resize(3, 0.); +// servo_scales = auto_exchange_->union_move_->getServoScale(); +// servo_command_sender_->setLinearVel(servo_scales[0], servo_scales[1], servo_scales[2]); +// } +// else +// servo_command_sender_->setZero(); +// } + ChassisGimbalManual::updatePc(dbus_data); +} + +void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) +{ + ManualBase::dbusDataCallback(data); + chassis_cmd_sender_->updateRefereeStatus(referee_is_online_); + if (servo_mode_ == SERVO) + updateServo(data); +} +void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) +{ + ROS_INFO_STREAM("stone num: "<< stone_num_); + if (data->data == "-1" && stone_num_ != 0) + stone_num_--; + else + stone_num_++; + if (stone_num_ >= 3) + stone_num_--; + engineer_ui_.stone_num = std::to_string(stone_num_); +} +void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) +{ + gpio_state_.gpio_state = data->gpio_state; + if (gpio_state_.gpio_state[0]) + { + gripper_state_ = "open"; + engineer_ui_.gripper_state = "open"; + } + else + { + gripper_state_ = "close"; + engineer_ui_.gripper_state = "close"; + } } void EngineerManual::sendCommand(const ros::Time& time) @@ -204,17 +263,67 @@ void EngineerManual::sendCommand(const ros::Time& time) { chassis_cmd_sender_->sendChassisCommand(time, false); vel_cmd_sender_->sendCommand(time); + reversal_command_sender_->sendCommand(time); } if (servo_mode_ == SERVO) + { + changeSpeedMode(EXCHANGE); servo_command_sender_->sendCommand(time); - if (gimbal_mode_ == RATE) - gimbal_cmd_sender_->sendCommand(time); + if (gimbal_mode_ == RATE) + gimbal_cmd_sender_->sendCommand(time); + } +} +void EngineerManual::runStepQueue(const std::string& step_queue_name) +{ + reversal_motion_ = true; + rm_msgs::EngineerGoal goal; + goal.step_queue_name = step_queue_name; + if (action_client_.isServerConnected()) + { + if (operating_mode_ == MANUAL) + action_client_.sendGoal(goal, boost::bind(&EngineerManual::actionDoneCallback, this, _1, _2), + boost::bind(&EngineerManual::actionActiveCallback, this), + boost::bind(&EngineerManual::actionFeedbackCallback, this, _1)); + operating_mode_ = MIDDLEWARE; + } + else + ROS_ERROR("Can not connect to middleware"); } -void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) +void EngineerManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) { - servo_command_sender_->setLinearVel(dbus_data->ch_l_y, -dbus_data->ch_l_x, -dbus_data->wheel); - servo_command_sender_->setAngularVel(dbus_data->ch_r_x, dbus_data->ch_r_y, angular_z_scale_); +} +void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& state, + const rm_msgs::EngineerResultConstPtr& result) +{ + ROS_INFO("Finished in state [%s]", state.toString().c_str()); + ROS_INFO("Result: %i", result->finish); + ROS_INFO("Done %s", (prefix_ + root_).c_str()); + reversal_motion_ = false; + change_flag_ = true; + engineer_ui_.stone_num = stone_num_; + ROS_INFO("%i", result->finish); + + operating_mode_ = MANUAL; +} + +void EngineerManual::enterServo() +{ + servo_mode_ = SERVO; + gimbal_mode_ = DIRECT; + changeSpeedMode(EXCHANGE); + servo_reset_caller_->callService(); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + action_client_.cancelAllGoals(); + chassis_cmd_sender_->getMsg()->command_source_frame = "link4"; +} +void EngineerManual::initMode() +{ + servo_mode_ = JOINT; + gimbal_mode_ = DIRECT; + changeSpeedMode(NORMAL); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; } void EngineerManual::remoteControlTurnOff() @@ -222,155 +331,134 @@ void EngineerManual::remoteControlTurnOff() ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } - +void EngineerManual::gimbalOutputOn() +{ + ChassisGimbalManual::gimbalOutputOn(); + pitch_calibration_->reset(); + ROS_INFO("pitch calibrated"); +} void EngineerManual::chassisOutputOn() { - power_on_calibration_->reset(); - if (MIDDLEWARE) + if (operating_mode_ == MIDDLEWARE) action_client_.cancelAllGoals(); } -void EngineerManual::rightSwitchDownRise() +//-------------------controller input------------------- +void EngineerManual::rightSwitchUpRise() { - ChassisGimbalManual::rightSwitchDownRise(); - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - servo_mode_ = SERVO; + ChassisGimbalManual::rightSwitchUpRise(); gimbal_mode_ = DIRECT; - servo_reset_caller_->callService(); - action_client_.cancelAllGoals(); + servo_mode_ = JOINT; + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } - void EngineerManual::rightSwitchMidRise() { ChassisGimbalManual::rightSwitchMidRise(); servo_mode_ = JOINT; gimbal_mode_ = RATE; + gimbal_cmd_sender_->setZero(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } - -void EngineerManual::rightSwitchUpRise() +void EngineerManual::rightSwitchDownRise() { - ChassisGimbalManual::rightSwitchUpRise(); - gimbal_mode_ = DIRECT; + shiftVPress(); + ChassisGimbalManual::rightSwitchDownRise(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + servo_mode_ = SERVO; + gimbal_mode_ = RATE; + servo_reset_caller_->callService(); + action_client_.cancelAllGoals(); + ROS_INFO_STREAM(servo_mode_); } void EngineerManual::leftSwitchUpRise() { - arm_calibration_->reset(); - power_on_calibration_->reset(); - runStepQueue("OPEN_GRIPPER"); -} - -void EngineerManual::leftSwitchDownFall() -{ - runStepQueue("HOME1"); - runStepQueue("OPEN_GRIPPER"); - engineer_ui_.step_queue_name = "HOME1"; + prefix_ = ""; + root_ = "CALIBRATION"; + calibration_gather_->reset(); + ROS_INFO_STREAM("START CALIBRATE"); } - void EngineerManual::leftSwitchUpFall() { + runStepQueue("HOME_ZERO_STONE"); + runStepQueue("CLOSE_GRIPPER"); } -void EngineerManual::runStepQueue(const std::string& step_queue_name) +void EngineerManual::leftSwitchDownRise() { - rm_msgs::EngineerGoal goal; - goal.step_queue_name = step_queue_name; - if (action_client_.isServerConnected()) - { - if (operating_mode_ == MANUAL) - action_client_.sendGoal(goal, boost::bind(&EngineerManual::actionDoneCallback, this, _1, _2), - boost::bind(&EngineerManual::actionActiveCallback, this), - boost::bind(&EngineerManual::actionFeedbackCallback, this, _1)); - operating_mode_ = MIDDLEWARE; - } - else - ROS_ERROR("Can not connect to middleware"); + runStepQueue("HOME_ZERO_STONE"); } - -void EngineerManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) -{ - engineer_ui_.current_step_name = feedback->current_step; - engineer_ui_.total_steps = feedback->total_steps; -} - -void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& state, - const rm_msgs::EngineerResultConstPtr& result) +void EngineerManual::leftSwitchDownFall() { - ROS_INFO("Finished in state [%s]", state.toString().c_str()); - ROS_INFO("Result: %i", result->finish); - ROS_INFO("Done %s", (prefix_ + root_).c_str()); - engineer_ui_.step_queue_name += " done!"; - operating_mode_ = MANUAL; + runStepQueue("HOME_ZERO_STONE"); + drag_state_ = "off"; + engineer_ui_.drag_state = "off"; } +//mouse input void EngineerManual::mouseLeftRelease() { - root_ += "0"; - engineer_ui_.step_queue_name = prefix_ + root_; - runStepQueue(prefix_ + root_); - ROS_INFO("Finished %s", (prefix_ + root_).c_str()); + if (change_flag_) + { + root_ += "0"; + change_flag_ = false; + runStepQueue(prefix_ + root_); + ROS_INFO("Finished %s", (prefix_ + root_).c_str()); + } } void EngineerManual::mouseRightRelease() { - engineer_ui_.step_queue_name = prefix_ + root_; runStepQueue(prefix_ + root_); ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } -void EngineerManual::ctrlQPress() +//keyboard input +//------------------------- ctrl ------------------------------ +void EngineerManual::ctrlAPress() { - prefix_ = "LF_"; + prefix_ = "MID_"; root_ = "SMALL_ISLAND"; - engineer_ui_.step_queue_name = prefix_ + root_; + runStepQueue(prefix_+root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } -void EngineerManual::ctrlWPress() +void EngineerManual::ctrlBPress() { - prefix_ = "SKY_"; - root_ = "BIG_ISLAND"; - engineer_ui_.step_queue_name = prefix_ + root_; - ROS_INFO("%s", (prefix_ + root_).c_str()); -} + switch (stone_num_) + { + case 0: + root_ = "_ZERO_STONE"; + break; + case 1: + root_ = "_ONE_STONE"; + break; + case 2: + root_ = "_TWO_STONE"; + break; + } + ROS_INFO("RUN_HOME"); -void EngineerManual::ctrlEPress() -{ - prefix_ = "RT_"; - root_ = "SMALL_ISLAND"; - engineer_ui_.step_queue_name = prefix_ + root_; - ROS_INFO("%s", (prefix_ + root_).c_str()); + prefix_ = "HOME"; + runStepQueue(prefix_ + root_); } -void EngineerManual::ctrlRPress() +void EngineerManual::ctrlCPress() { - arm_calibration_->reset(); - engineer_ui_.step_queue_name = "calibration"; - ROS_INFO("Calibrated"); + action_client_.cancelAllGoals(); } -void EngineerManual::ctrlAPress() +void EngineerManual::ctrlDPress() { - prefix_ = ""; + prefix_ = "R_"; root_ = "SMALL_ISLAND"; - engineer_ui_.step_queue_name = prefix_ + root_; - ROS_INFO("%s", (prefix_ + root_).c_str()); -} - -void EngineerManual::ctrlSPress() -{ - prefix_ = ""; - root_ = "BIG_ISLAND"; - engineer_ui_.step_queue_name = prefix_ + root_; + runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } -void EngineerManual::ctrlDPress() +void EngineerManual::ctrlEPress() { - prefix_ = ""; - root_ = "GROUND_STONE"; - runStepQueue(root_); - engineer_ui_.step_queue_name = prefix_ + root_; + prefix_ = "L_"; + root_ = "SMALL_ISLAND"; + runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -379,7 +467,6 @@ void EngineerManual::ctrlFPress() prefix_ = ""; root_ = "EXCHANGE_WAIT"; runStepQueue(root_); - engineer_ui_.step_queue_name = prefix_ + root_; ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -388,32 +475,43 @@ void EngineerManual::ctrlGPress() switch (stone_num_) { case 0: - root_ = "STORE_STONE0"; + root_ = "STORE_WHEN_ZERO_STONE"; stone_num_ = 1; break; case 1: - root_ = "STORE_STONE1"; + root_ = "STORE_WHEN_ONE_STONE"; stone_num_ = 2; break; } runStepQueue(root_); prefix_ = ""; - engineer_ui_.step_queue_name = prefix_ + root_; + ROS_INFO("STORE_STONE"); } -void EngineerManual::ctrlZPress() +void EngineerManual::ctrlQPress() { + prefix_ = "L_"; + root_ = "SMALL_ISLAND"; + ROS_INFO("%s", (prefix_ + root_).c_str()); } -void EngineerManual::ctrlXPress() +void EngineerManual::ctrlRPress() { + prefix_ = ""; + root_ = "CALIBRATION"; + servo_mode_ = JOINT; + calibration_gather_->reset(); + runStepQueue("CLOSE_GRIPPER"); + ROS_INFO_STREAM("START CALIBRATE"); } -void EngineerManual::ctrlCPress() +void EngineerManual::ctrlSPress() { - action_client_.cancelAllGoals(); - engineer_ui_.step_queue_name = "cancel"; + prefix_ = ""; + root_ = "BIG_ISLAND"; + + ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlVPress() @@ -421,113 +519,105 @@ void EngineerManual::ctrlVPress() if (state_) { runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.step_queue_name = "CLOSE_GRIPPER"; state_ = false; } else if (!state_) { runStepQueue("OPEN_GRIPPER"); - engineer_ui_.step_queue_name = "OPEN_GRIPPER"; state_ = true; } } - void EngineerManual::ctrlVRelease() { } -void EngineerManual::ctrlBPress() +void EngineerManual::ctrlWPress() { - switch (stone_num_) - { - case 0: - root_ = "HOME0"; - break; - case 1: - root_ = "HOME1"; - break; - case 2: - root_ = "HOME2"; - break; - } - ROS_INFO("RUN_HOME"); - engineer_ui_.step_queue_name = prefix_ + root_; - prefix_ = ""; - runStepQueue(root_); + prefix_ = "SKY_"; + root_ = "BIG_ISLAND"; + ROS_INFO("%s", (prefix_ + root_).c_str()); } -void EngineerManual::qPressing() +void EngineerManual::ctrlXPress() { - vel_cmd_sender_->setAngularZVel(gyro_scale_); } -void EngineerManual::qRelease() +void EngineerManual::ctrlZPress() +{ +} +//-------------------------- keys ------------------------------ +void EngineerManual::bPressing() +{ +} +void EngineerManual::bRelease() { - vel_cmd_sender_->setAngularZVel(0); +} + +void EngineerManual::cPressing() +{ + angular_z_scale_ = -0.1; +} +void EngineerManual::cRelease() +{ + angular_z_scale_ = 0.; } void EngineerManual::ePressing() { vel_cmd_sender_->setAngularZVel(-gyro_scale_); } - void EngineerManual::eRelease() { vel_cmd_sender_->setAngularZVel(0); } -void EngineerManual::zPressing() +void EngineerManual::fPress() { - angular_z_scale_ = 0.1; } - -void EngineerManual::zRelease() +void EngineerManual::fRelease() { - angular_z_scale_ = 0.; } -void EngineerManual::cPressing() +void EngineerManual::gPress() { - angular_z_scale_ = -0.1; } - -void EngineerManual::cRelease() +void EngineerManual::gRelease() { - angular_z_scale_ = 0.; } -void EngineerManual::rPress() +void EngineerManual::qPressing() { + vel_cmd_sender_->setAngularZVel(gyro_scale_); } - -void EngineerManual::vPressing() +void EngineerManual::qRelease() { + vel_cmd_sender_->setAngularZVel(0); } -void EngineerManual::vRelease() +void EngineerManual::rPress() { } -void EngineerManual::bPressing() +void EngineerManual::vPressing() { } - -void EngineerManual::bRelease() +void EngineerManual::vRelease() { } -void EngineerManual::gPressing() -{ -} -void EngineerManual::gRelease() +void EngineerManual::xPress() { } -void EngineerManual::fPressing() + +void EngineerManual::zPressing() { + angular_z_scale_ = 0.1; } -void EngineerManual::fRelease() +void EngineerManual::zRelease() { + angular_z_scale_ = 0.; } +//----------------------------- shift ---------------------------- void EngineerManual::shiftPressing() { changeSpeedMode(FAST); @@ -536,69 +626,36 @@ void EngineerManual::shiftRelease() { changeSpeedMode(NORMAL); } -void EngineerManual::shiftRPress() + +void EngineerManual::shiftBPress() { - runStepQueue("SKY_GIMBAL"); - engineer_ui_.step_queue_name = "gimbal SKY_GIMBAL"; - ROS_INFO("enter gimbal SKY_GIMBAL"); + runStepQueue("TEMP_GIMBAL"); + ROS_INFO("enter gimbal BACK_GIMBAL"); +} +void EngineerManual::shiftBRelease() +{ + runStepQueue("BACK_GIMBAL"); } + void EngineerManual::shiftCPress() { if (servo_mode_) { servo_mode_ = false; - engineer_ui_.step_queue_name = "ENTER servo"; ROS_INFO("EXIT SERVO"); } else { servo_mode_ = true; - engineer_ui_.step_queue_name = "exit SERVO"; ROS_INFO("ENTER SERVO"); } ROS_INFO("cancel all goal"); } -void EngineerManual::shiftZPress() -{ - runStepQueue("REVERSAL_GIMBAL"); - ROS_INFO("enter gimbal REVERSAL_GIMBAL"); - engineer_ui_.step_queue_name = "gimbal REVERSAL_GIMBAL"; -} -void EngineerManual::shiftVPress() -{ - // gimbal - gimbal_mode_ = RATE; - ROS_INFO("MANUAL_VIEW"); - engineer_ui_.step_queue_name = "gimbal MANUAL_VIEW"; -} -void EngineerManual::shiftVRelease() +void EngineerManual::shiftFPress() { - // gimbal - gimbal_mode_ = DIRECT; - ROS_INFO("DIRECT"); - engineer_ui_.step_queue_name = "gimbal DIRECT"; -} - -void EngineerManual::shiftBPress() -{ - runStepQueue("TEMP_GIMBAL"); - ROS_INFO("enter gimbal BACK_GIMBAL"); - engineer_ui_.step_queue_name = "gimbal BACK_GIMBAL"; -} - -void EngineerManual::shiftBRelease() -{ - runStepQueue("BACK_GIMBAL"); -} - -void EngineerManual::shiftXPress() -{ - runStepQueue("GROUND_GIMBAL"); - ROS_INFO("enter gimbal GROUND_GIMBAL"); - engineer_ui_.step_queue_name = "gimbal GROUND_GIMBAL"; } void EngineerManual::shiftGPress() @@ -606,23 +663,63 @@ void EngineerManual::shiftGPress() switch (stone_num_) { case 0: - root_ = "NO!!"; + root_ = "NO STONE!!"; stone_num_ = 0; break; case 1: - root_ = "TAKE_STONE0"; + root_ = "TAKE_WHEN_ONE_STONE"; stone_num_ = 0; break; case 2: - root_ = "TAKE_STONE1"; + root_ = "TAKE_WHEN_TWO_STONE"; stone_num_ = 1; break; } runStepQueue(root_); prefix_ = ""; - engineer_ui_.step_queue_name = "TAKE_STONE"; ROS_INFO("TAKE_STONE"); } +void EngineerManual::shiftRPress() +{ + runStepQueue("SKY_GIMBAL"); + ROS_INFO("enter gimbal SKY_GIMBAL"); +} +void EngineerManual::shiftRRelease() +{ +} + +void EngineerManual::shiftVPress() +{ + if (gripper_state_ == "close") + { + runStepQueue("CLOSE_GRIPPER"); + engineer_ui_.gripper_state = "open"; + } + else + { + runStepQueue("OPEN_GRIPPER"); + engineer_ui_.gripper_state = "close"; + } +} +void EngineerManual::shiftVRelease() +{ + // gimbal + gimbal_mode_ = DIRECT; + ROS_INFO("DIRECT"); +} + +void EngineerManual::shiftXPress() +{ + runStepQueue("GROUND_GIMBAL"); + ROS_INFO("enter gimbal GROUND_GIMBAL"); +} + +void EngineerManual::shiftZPress() +{ + runStepQueue("REVERSAL_GIMBAL"); + ROS_INFO("enter gimbal REVERSAL_GIMBAL"); +} + } // namespace rm_manual From 41b6d64e074e675fe165a09257c4469c792c9e3a Mon Sep 17 00:00:00 2001 From: =cc0h <=792166012@qq.com> Date: Wed, 17 Jan 2024 10:45:25 +0800 Subject: [PATCH 02/14] Add command sender. --- include/rm_manual/engineer_manual.h | 6 +++--- src/engineer_manual.cpp | 24 +++++++++++++----------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 289ef312..aef99033 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -140,7 +140,7 @@ class EngineerManual : public ChassisGimbalManual - bool reversal_motion_{}, change_flag_{}; + 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_{}; @@ -156,9 +156,9 @@ class EngineerManual : public ChassisGimbalManual rm_msgs::EngineerUi engineer_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; - rm_common::MultiDofCommandSender* reversal_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; - rm_common::JointPositionBinaryCommandSender *drag_command_sender_; + 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 action_client_; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 60a9f117..7d5b4bce 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -41,9 +41,18 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); - ros::NodeHandle nh_reversal(nh, "reversal"); - reversal_command_sender_ = new rm_common::MultiDofCommandSender(nh_reversal); - + //Extend arm + ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); + ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); + extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); + extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); + //Ore bin + ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); + ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); + ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); + ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); + ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); + gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); // Calibration XmlRpc::XmlRpcValue rpc_value; nh.getParam("pitch_calibration", rpc_value); @@ -198,9 +207,6 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) checkKeyboard(dbus_data); left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - if (!reversal_motion_ && servo_mode_ == JOINT) - reversal_command_sender_->setGroupValue(0., 0., 5 * dbus_data->ch_r_y, 5 * dbus_data->ch_l_x, 5 * dbus_data->ch_l_y, - 0.); } void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { @@ -263,7 +269,6 @@ void EngineerManual::sendCommand(const ros::Time& time) { chassis_cmd_sender_->sendChassisCommand(time, false); vel_cmd_sender_->sendCommand(time); - reversal_command_sender_->sendCommand(time); } if (servo_mode_ == SERVO) { @@ -275,7 +280,6 @@ void EngineerManual::sendCommand(const ros::Time& time) } void EngineerManual::runStepQueue(const std::string& step_queue_name) { - reversal_motion_ = true; rm_msgs::EngineerGoal goal; goal.step_queue_name = step_queue_name; if (action_client_.isServerConnected()) @@ -299,7 +303,6 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& ROS_INFO("Finished in state [%s]", state.toString().c_str()); ROS_INFO("Result: %i", result->finish); ROS_INFO("Done %s", (prefix_ + root_).c_str()); - reversal_motion_ = false; change_flag_ = true; engineer_ui_.stone_num = stone_num_; ROS_INFO("%i", result->finish); @@ -718,8 +721,7 @@ void EngineerManual::shiftXPress() void EngineerManual::shiftZPress() { - runStepQueue("REVERSAL_GIMBAL"); - ROS_INFO("enter gimbal REVERSAL_GIMBAL"); + } } // namespace rm_manual From c934b29a1b0edfe772549a9f8c53c741fb6f2846 Mon Sep 17 00:00:00 2001 From: =cc0h <=792166012@qq.com> Date: Sat, 24 Feb 2024 19:12:22 +0800 Subject: [PATCH 03/14] record. --- src/engineer_manual.cpp | 92 +++++++++++++++-------------------------- 1 file changed, 33 insertions(+), 59 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 7d5b4bce..7d00caee 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -16,9 +16,9 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) action_client_.waitForServer(); ROS_INFO("Middleware started."); // Auto Exchange - XmlRpc::XmlRpcValue auto_exchange_value; - nh.getParam("auto_exchange", auto_exchange_value); - ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); +// XmlRpc::XmlRpcValue auto_exchange_value; +// nh.getParam("auto_exchange", auto_exchange_value); +// ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); // auto_exchange_ = new auto_exchange::AutoExchange(auto_exchange_value, tf_buffer_, nh_auto_exchange); // Pub // exchanger_update_pub_ = nh.advertise("/is_update_exchanger", 1); @@ -42,17 +42,17 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); //Extend arm - ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); - ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); - extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); - extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); - //Ore bin - ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); - ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); - ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); - ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); - ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); - gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); +// ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); +// ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); +// extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); +// extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); +// //Ore bin +// ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); +// ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); +// ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); +// ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); +// ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); +// gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); // Calibration XmlRpc::XmlRpcValue rpc_value; nh.getParam("pitch_calibration", rpc_value); @@ -210,24 +210,8 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) } void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { -// if (auto_exchange_->union_move_->getEnterFlag() != true && !auto_exchange_->getFinishFlag()) -// { - servo_command_sender_->setLinearVel(dbus_data->wheel, -dbus_data->ch_l_x, dbus_data->ch_l_y); - servo_command_sender_->setAngularVel(-angular_z_scale_, dbus_data->ch_r_y, dbus_data->ch_r_x); -// } -// else -// { -// if (!auto_exchange_->union_move_->getFinishFlag()) -// { -// std::vector servo_scales; -// servo_scales.resize(3, 0.); -// servo_scales = auto_exchange_->union_move_->getServoScale(); -// servo_command_sender_->setLinearVel(servo_scales[0], servo_scales[1], servo_scales[2]); -// } -// else -// servo_command_sender_->setZero(); -// } - ChassisGimbalManual::updatePc(dbus_data); + servo_command_sender_->setLinearVel(dbus_data->wheel, -dbus_data->ch_l_x, dbus_data->ch_l_y); + servo_command_sender_->setAngularVel(angular_z_scale_, dbus_data->ch_r_y, dbus_data->ch_r_x); } void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) @@ -364,14 +348,13 @@ void EngineerManual::rightSwitchMidRise() } void EngineerManual::rightSwitchDownRise() { - shiftVPress(); ChassisGimbalManual::rightSwitchDownRise(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); servo_mode_ = SERVO; gimbal_mode_ = RATE; servo_reset_caller_->callService(); action_client_.cancelAllGoals(); - ROS_INFO_STREAM(servo_mode_); + ROS_INFO_STREAM("servo_mode"); } void EngineerManual::leftSwitchUpRise() @@ -394,8 +377,6 @@ void EngineerManual::leftSwitchDownRise() void EngineerManual::leftSwitchDownFall() { runStepQueue("HOME_ZERO_STONE"); - drag_state_ = "off"; - engineer_ui_.drag_state = "off"; } //mouse input void EngineerManual::mouseLeftRelease() @@ -468,7 +449,7 @@ void EngineerManual::ctrlEPress() void EngineerManual::ctrlFPress() { prefix_ = ""; - root_ = "EXCHANGE_WAIT"; + root_ = "EXCHANGE_POS"; runStepQueue(root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -519,16 +500,16 @@ void EngineerManual::ctrlSPress() void EngineerManual::ctrlVPress() { - if (state_) - { - runStepQueue("CLOSE_GRIPPER"); - state_ = false; - } - else if (!state_) - { - runStepQueue("OPEN_GRIPPER"); - state_ = true; - } + if (gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + engineer_ui_.gripper_state = "open"; + } + else + { + runStepQueue("CLOSE_GRIPPER"); + engineer_ui_.gripper_state = "close"; + } } void EngineerManual::ctrlVRelease() { @@ -558,7 +539,8 @@ void EngineerManual::bRelease() void EngineerManual::cPressing() { - angular_z_scale_ = -0.1; + angular_z_scale_ = 0.5; + ROS_INFO_STREAM("angular_z_scale is 0.5"); } void EngineerManual::cRelease() { @@ -614,7 +596,8 @@ void EngineerManual::xPress() void EngineerManual::zPressing() { - angular_z_scale_ = 0.1; + angular_z_scale_ = -0.5; + ROS_INFO_STREAM("angular_z_scale is -0.5"); } void EngineerManual::zRelease() { @@ -695,16 +678,7 @@ void EngineerManual::shiftRRelease() void EngineerManual::shiftVPress() { - if (gripper_state_ == "close") - { - runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state = "open"; - } - else - { - runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state = "close"; - } + } void EngineerManual::shiftVRelease() { From fd059d2479fb82d3b5ae4b2783ed8df116681c1e Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 26 Mar 2024 21:17:27 +0800 Subject: [PATCH 04/14] Record. --- include/rm_manual/engineer_manual.h | 30 +++---- src/engineer_manual.cpp | 129 ++++++++++++---------------- 2 files changed, 69 insertions(+), 90 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index aef99033..116120b0 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -134,19 +134,14 @@ class EngineerManual : public ChassisGimbalManual void mouseLeftRelease(); void mouseRightRelease(); - // 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_{}; + 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_{}, drag_state_{ "off" }, gripper_state_{"off"}; - int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{0}; + std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }; ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; @@ -158,19 +153,18 @@ class EngineerManual : public ChassisGimbalManual rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; 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::JointPointCommandSender *ore_bin_lifter_command_sender_, *ore_bin_rotate_command_sender_, + *gimbal_lifter_command_sender_; rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_; actionlib::SimpleActionClient 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_; + 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_; }; } // namespace rm_manual diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 7d00caee..f154dca8 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -16,12 +16,12 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) action_client_.waitForServer(); ROS_INFO("Middleware started."); // Auto Exchange -// XmlRpc::XmlRpcValue auto_exchange_value; -// nh.getParam("auto_exchange", auto_exchange_value); -// ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); -// auto_exchange_ = new auto_exchange::AutoExchange(auto_exchange_value, tf_buffer_, nh_auto_exchange); + // XmlRpc::XmlRpcValue auto_exchange_value; + // nh.getParam("auto_exchange", auto_exchange_value); + // ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); + // auto_exchange_ = new auto_exchange::AutoExchange(auto_exchange_value, tf_buffer_, nh_auto_exchange); // Pub -// exchanger_update_pub_ = nh.advertise("/is_update_exchanger", 1); + // exchanger_update_pub_ = nh.advertise("/is_update_exchanger", 1); // Sub stone_num_sub_ = nh.subscribe("/stone_num", 10, &EngineerManual::stoneNumCallback, this); gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, @@ -36,23 +36,23 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) chassis_nh.param("fast_speed_scale", fast_speed_scale_, 1.0); chassis_nh.param("normal_speed_scale", normal_speed_scale_, 0.5); chassis_nh.param("low_speed_scale", low_speed_scale_, 0.3); - chassis_nh.param("exchange_speed_scale", exchange_speed_scale_, 0.3); + chassis_nh.param("exchange_speed_scale", exchange_speed_scale_, 0.2); chassis_nh.param("fast_gyro_scale", fast_gyro_scale_, 0.5); chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); - //Extend arm -// ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); -// ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); -// extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); -// extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); -// //Ore bin -// ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); -// ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); -// ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); -// ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); -// ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); -// gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); + // Extend arm + // ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); + // ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); + // extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); + // extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); + // //Ore bin + ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); + ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); + ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); + ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); + ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); + gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); // Calibration XmlRpc::XmlRpcValue rpc_value; nh.getParam("pitch_calibration", rpc_value); @@ -94,7 +94,7 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) r_event_.setRising(boost::bind(&EngineerManual::rPress, this)); v_event_.setActiveHigh(boost::bind(&EngineerManual::vPressing, this)); v_event_.setFalling(boost::bind(&EngineerManual::vRelease, this)); - x_event_.setRising(boost::bind(&EngineerManual::xPress,this)); + x_event_.setRising(boost::bind(&EngineerManual::xPress, this)); z_event_.setActiveHigh(boost::bind(&EngineerManual::zPressing, this)); z_event_.setFalling(boost::bind(&EngineerManual::zRelease, this)); shift_event_.setActiveHigh(boost::bind(&EngineerManual::shiftPressing, this)); @@ -189,7 +189,6 @@ void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); } - void EngineerManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::updateRc(dbus_data); @@ -207,13 +206,13 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) checkKeyboard(dbus_data); left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + vel_cmd_sender_->setAngularZVel(dbus_data->m_x * gimbal_scale_); } void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { servo_command_sender_->setLinearVel(dbus_data->wheel, -dbus_data->ch_l_x, dbus_data->ch_l_y); servo_command_sender_->setAngularVel(angular_z_scale_, dbus_data->ch_r_y, dbus_data->ch_r_x); } - void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { ManualBase::dbusDataCallback(data); @@ -223,14 +222,13 @@ void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) } void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { - ROS_INFO_STREAM("stone num: "<< stone_num_); if (data->data == "-1" && stone_num_ != 0) stone_num_--; else stone_num_++; - if (stone_num_ >= 3) + if (stone_num_ >= 2) stone_num_--; - engineer_ui_.stone_num = std::to_string(stone_num_); + ROS_INFO("RUN_HOME"); } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -238,15 +236,12 @@ void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) if (gpio_state_.gpio_state[0]) { gripper_state_ = "open"; - engineer_ui_.gripper_state = "open"; } else { gripper_state_ = "close"; - engineer_ui_.gripper_state = "close"; } } - void EngineerManual::sendCommand(const ros::Time& time) { if (operating_mode_ == MANUAL) @@ -277,7 +272,6 @@ void EngineerManual::runStepQueue(const std::string& step_queue_name) else ROS_ERROR("Can not connect to middleware"); } - void EngineerManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) { } @@ -288,9 +282,7 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& ROS_INFO("Result: %i", result->finish); ROS_INFO("Done %s", (prefix_ + root_).c_str()); change_flag_ = true; - engineer_ui_.stone_num = stone_num_; ROS_INFO("%i", result->finish); - operating_mode_ = MANUAL; } @@ -306,11 +298,11 @@ void EngineerManual::enterServo() } void EngineerManual::initMode() { - servo_mode_ = JOINT; - gimbal_mode_ = DIRECT; - changeSpeedMode(NORMAL); - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; + servo_mode_ = JOINT; + gimbal_mode_ = DIRECT; + changeSpeedMode(NORMAL); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; } void EngineerManual::remoteControlTurnOff() @@ -372,13 +364,19 @@ void EngineerManual::leftSwitchUpFall() void EngineerManual::leftSwitchDownRise() { - runStepQueue("HOME_ZERO_STONE"); + if (gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + } + else + { + runStepQueue("CLOSE_GRIPPER"); + } } void EngineerManual::leftSwitchDownFall() { - runStepQueue("HOME_ZERO_STONE"); } -//mouse input +// mouse input void EngineerManual::mouseLeftRelease() { if (change_flag_) @@ -395,13 +393,14 @@ void EngineerManual::mouseRightRelease() runStepQueue(prefix_ + root_); ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } -//keyboard input +//--------------------- keyboard input ------------------------ + //------------------------- ctrl ------------------------------ void EngineerManual::ctrlAPress() { prefix_ = "MID_"; root_ = "SMALL_ISLAND"; - runStepQueue(prefix_+root_); + runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -415,19 +414,16 @@ void EngineerManual::ctrlBPress() case 1: root_ = "_ONE_STONE"; break; - case 2: - root_ = "_TWO_STONE"; - break; } - ROS_INFO("RUN_HOME"); - prefix_ = "HOME"; runStepQueue(prefix_ + root_); + ROS_INFO("RUN_HOME"); } void EngineerManual::ctrlCPress() { action_client_.cancelAllGoals(); + ROS_INFO("cancel all goal"); } void EngineerManual::ctrlDPress() @@ -452,6 +448,7 @@ void EngineerManual::ctrlFPress() root_ = "EXCHANGE_POS"; runStepQueue(root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + enterServo(); } void EngineerManual::ctrlGPress() @@ -500,16 +497,14 @@ void EngineerManual::ctrlSPress() void EngineerManual::ctrlVPress() { - if (gripper_state_ == "close") - { - runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state = "open"; - } - else - { - runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state = "close"; - } + if (gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + } + else + { + runStepQueue("CLOSE_GRIPPER"); + } } void EngineerManual::ctrlVRelease() { @@ -517,8 +512,8 @@ void EngineerManual::ctrlVRelease() void EngineerManual::ctrlWPress() { - prefix_ = "SKY_"; - root_ = "BIG_ISLAND"; + prefix_ = ""; + root_ = ""; ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -540,7 +535,7 @@ void EngineerManual::bRelease() void EngineerManual::cPressing() { angular_z_scale_ = 0.5; - ROS_INFO_STREAM("angular_z_scale is 0.5"); + ROS_INFO_STREAM("angular_z_scale is 0.5"); } void EngineerManual::cRelease() { @@ -549,11 +544,9 @@ void EngineerManual::cRelease() void EngineerManual::ePressing() { - vel_cmd_sender_->setAngularZVel(-gyro_scale_); } void EngineerManual::eRelease() { - vel_cmd_sender_->setAngularZVel(0); } void EngineerManual::fPress() @@ -572,11 +565,9 @@ void EngineerManual::gRelease() void EngineerManual::qPressing() { - vel_cmd_sender_->setAngularZVel(gyro_scale_); } void EngineerManual::qRelease() { - vel_cmd_sender_->setAngularZVel(0); } void EngineerManual::rPress() @@ -625,19 +616,20 @@ void EngineerManual::shiftBRelease() void EngineerManual::shiftCPress() { + action_client_.cancelAllGoals(); + ROS_INFO("cancel all goal"); if (servo_mode_) { servo_mode_ = false; - + initMode(); ROS_INFO("EXIT SERVO"); } else { servo_mode_ = true; - + enterServo(); ROS_INFO("ENTER SERVO"); } - ROS_INFO("cancel all goal"); } void EngineerManual::shiftFPress() @@ -678,24 +670,17 @@ void EngineerManual::shiftRRelease() void EngineerManual::shiftVPress() { - } void EngineerManual::shiftVRelease() { - // gimbal - gimbal_mode_ = DIRECT; - ROS_INFO("DIRECT"); } void EngineerManual::shiftXPress() { - runStepQueue("GROUND_GIMBAL"); - ROS_INFO("enter gimbal GROUND_GIMBAL"); } void EngineerManual::shiftZPress() { - } } // namespace rm_manual From 9c8f3cd184c5399e62578d87057ed983cd05836c Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 10 Apr 2024 11:53:56 +0800 Subject: [PATCH 05/14] Fix can't exit servo mode.Adjust step name.Control chassis rotate with qe and mouse in different mode. --- src/engineer_manual.cpp | 137 ++++++++++++++++++---------------------- 1 file changed, 63 insertions(+), 74 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index f154dca8..c76055e1 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -15,14 +15,6 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); - // Auto Exchange - // XmlRpc::XmlRpcValue auto_exchange_value; - // nh.getParam("auto_exchange", auto_exchange_value); - // ros::NodeHandle nh_auto_exchange(nh, "auto_exchange"); - // auto_exchange_ = new auto_exchange::AutoExchange(auto_exchange_value, tf_buffer_, nh_auto_exchange); - // Pub - // exchanger_update_pub_ = nh.advertise("/is_update_exchanger", 1); - // Sub stone_num_sub_ = nh.subscribe("/stone_num", 10, &EngineerManual::stoneNumCallback, this); gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, &EngineerManual::gpioStateCallback, this); @@ -41,18 +33,8 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); - // Extend arm - // ros::NodeHandle nh_extend_arm_a(nh, "extend_arm_a"); - // ros::NodeHandle nh_extend_arm_b(nh, "extend_arm_b"); - // extend_arm_a_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_a); - // extend_arm_b_command_sender_ = new rm_common::JointPositionBinaryCommandSender(nh_extend_arm_b); - // //Ore bin - ros::NodeHandle nh_ore_bin_lifter(nh, "ore_bin_lifter"); - ros::NodeHandle nh_ore_bin_rotator(nh, "ore_bin_rotator"); - ore_bin_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_lifter, joint_state_); - ore_bin_rotate_command_sender_ = new rm_common::JointPointCommandSender(nh_ore_bin_rotator, joint_state_); - ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); - gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); + // ros::NodeHandle nh_gimbal_lifter(nh, "gimbal_lifter"); + // gimbal_lifter_command_sender_ = new rm_common::JointPointCommandSender(nh_gimbal_lifter, joint_state_); // Calibration XmlRpc::XmlRpcValue rpc_value; nh.getParam("pitch_calibration", rpc_value); @@ -206,12 +188,13 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) checkKeyboard(dbus_data); left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - vel_cmd_sender_->setAngularZVel(dbus_data->m_x * gimbal_scale_); + if (servo_mode_ == JOINT) + vel_cmd_sender_->setAngularZVel(-dbus_data->m_x * gimbal_scale_); } void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { - servo_command_sender_->setLinearVel(dbus_data->wheel, -dbus_data->ch_l_x, dbus_data->ch_l_y); - servo_command_sender_->setAngularVel(angular_z_scale_, dbus_data->ch_r_y, dbus_data->ch_r_x); + servo_command_sender_->setLinearVel(-dbus_data->wheel, -dbus_data->ch_l_x, -dbus_data->ch_l_y); + servo_command_sender_->setAngularVel(-angular_z_scale_, dbus_data->ch_r_y, -dbus_data->ch_r_x); } void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { @@ -222,13 +205,10 @@ void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) } void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { - if (data->data == "-1" && stone_num_ != 0) + if (data->data == "-1" && stone_num_ > 0) stone_num_--; - else + else if (data->data == "+1" && stone_num_ < 2) stone_num_++; - if (stone_num_ >= 2) - stone_num_--; - ROS_INFO("RUN_HOME"); } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -284,6 +264,17 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& change_flag_ = true; ROS_INFO("%i", result->finish); operating_mode_ = MANUAL; + if (prefix_ + root_ == "EXCHANGE_POS" || prefix_ + root_ == "GET_DOWN_STONE_BIN" || + prefix_ + root_ == "GET_UP_STONE_BIN") + { + enterServo(); + changeSpeedMode(EXCHANGE); + } + if (prefix_ == "HOME_") + { + initMode(); + changeSpeedMode(NORMAL); + } } void EngineerManual::enterServo() @@ -398,7 +389,7 @@ void EngineerManual::mouseRightRelease() //------------------------- ctrl ------------------------------ void EngineerManual::ctrlAPress() { - prefix_ = "MID_"; + prefix_ = "ONE_STONE_"; root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -406,18 +397,21 @@ void EngineerManual::ctrlAPress() void EngineerManual::ctrlBPress() { + prefix_ = "HOME_"; switch (stone_num_) { case 0: - root_ = "_ZERO_STONE"; + root_ = "ZERO_STONE"; break; case 1: - root_ = "_ONE_STONE"; + root_ = "ONE_STONE"; + break; + case 2: + root_ = "TWO_STONE"; break; } - prefix_ = "HOME"; runStepQueue(prefix_ + root_); - ROS_INFO("RUN_HOME"); + ROS_INFO_STREAM("RUN_HOME" << stone_num_); } void EngineerManual::ctrlCPress() @@ -428,7 +422,7 @@ void EngineerManual::ctrlCPress() void EngineerManual::ctrlDPress() { - prefix_ = "R_"; + prefix_ = "TWO_STONE_"; root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -436,10 +430,6 @@ void EngineerManual::ctrlDPress() void EngineerManual::ctrlEPress() { - prefix_ = "L_"; - root_ = "SMALL_ISLAND"; - runStepQueue(prefix_ + root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlFPress() @@ -448,33 +438,29 @@ void EngineerManual::ctrlFPress() root_ = "EXCHANGE_POS"; runStepQueue(root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - enterServo(); } void EngineerManual::ctrlGPress() { - switch (stone_num_) - { - case 0: - root_ = "STORE_WHEN_ZERO_STONE"; - stone_num_ = 1; - break; - case 1: - root_ = "STORE_WHEN_ONE_STONE"; - stone_num_ = 2; - break; - } - runStepQueue(root_); - prefix_ = ""; + // switch (stone_num_) + // { + // case 0: + // root_ = "STORE_WHEN_ZERO_STONE"; + // stone_num_ = 1; + // break; + // case 1: + // root_ = "DOWN_STONE_FROM_BIN"; + // stone_num_ = 2; + // break; + // } + // runStepQueue(root_); + // prefix_ = ""; - ROS_INFO("STORE_STONE"); + // ROS_INFO("STORE_STONE"); } void EngineerManual::ctrlQPress() { - prefix_ = "L_"; - root_ = "SMALL_ISLAND"; - ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlRPress() @@ -489,9 +475,9 @@ void EngineerManual::ctrlRPress() void EngineerManual::ctrlSPress() { - prefix_ = ""; + prefix_ = "MID_"; root_ = "BIG_ISLAND"; - + runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } @@ -512,9 +498,6 @@ void EngineerManual::ctrlVRelease() void EngineerManual::ctrlWPress() { - prefix_ = ""; - root_ = ""; - ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlXPress() @@ -544,9 +527,13 @@ void EngineerManual::cRelease() void EngineerManual::ePressing() { + if (servo_mode_ == SERVO) + vel_cmd_sender_->setAngularZVel(-gyro_scale_); } void EngineerManual::eRelease() { + if (servo_mode_ == SERVO) + vel_cmd_sender_->setAngularZVel(0); } void EngineerManual::fPress() @@ -565,13 +552,22 @@ void EngineerManual::gRelease() void EngineerManual::qPressing() { + if (servo_mode_ == SERVO) + vel_cmd_sender_->setAngularZVel(gyro_scale_); } void EngineerManual::qRelease() { + if (servo_mode_ == SERVO) + vel_cmd_sender_->setAngularZVel(0); } void EngineerManual::rPress() { + if (stone_num_ < 2) + stone_num_++; + else + stone_num_ = 0; + ROS_INFO_STREAM("Stone num is: " << stone_num_); } void EngineerManual::vPressing() @@ -598,38 +594,35 @@ void EngineerManual::zRelease() void EngineerManual::shiftPressing() { changeSpeedMode(FAST); + ROS_DEBUG_STREAM("speed mode is fast"); } void EngineerManual::shiftRelease() { changeSpeedMode(NORMAL); + ROS_DEBUG_STREAM("speed mode is normal"); } void EngineerManual::shiftBPress() { - runStepQueue("TEMP_GIMBAL"); - ROS_INFO("enter gimbal BACK_GIMBAL"); } void EngineerManual::shiftBRelease() { - runStepQueue("BACK_GIMBAL"); } void EngineerManual::shiftCPress() { action_client_.cancelAllGoals(); - ROS_INFO("cancel all goal"); - if (servo_mode_) + if (servo_mode_ == SERVO) { - servo_mode_ = false; initMode(); ROS_INFO("EXIT SERVO"); } else { - servo_mode_ = true; enterServo(); ROS_INFO("ENTER SERVO"); } + ROS_INFO("cancel all goal"); } void EngineerManual::shiftFPress() @@ -645,12 +638,10 @@ void EngineerManual::shiftGPress() stone_num_ = 0; break; case 1: - root_ = "TAKE_WHEN_ONE_STONE"; - stone_num_ = 0; + root_ = "GET_DOWN_STONE_BIN"; break; case 2: - root_ = "TAKE_WHEN_TWO_STONE"; - stone_num_ = 1; + root_ = "GET_UP_STONE_BIN"; break; } runStepQueue(root_); @@ -661,8 +652,6 @@ void EngineerManual::shiftGPress() void EngineerManual::shiftRPress() { - runStepQueue("SKY_GIMBAL"); - ROS_INFO("enter gimbal SKY_GIMBAL"); } void EngineerManual::shiftRRelease() { From 452449eddf6c28855d0c174ace1bff1ea8ba24c2 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 11 Apr 2024 04:52:44 +0800 Subject: [PATCH 06/14] Add engineer ui. --- src/engineer_manual.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index c76055e1..a1f9b95a 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -209,6 +209,7 @@ void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) stone_num_--; else if (data->data == "+1" && stone_num_ < 2) stone_num_++; + engineer_ui_.stone_num = stone_num_; } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -216,10 +217,12 @@ void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) if (gpio_state_.gpio_state[0]) { gripper_state_ = "open"; + engineer_ui_.gripper_state = "open"; } else { gripper_state_ = "close"; + engineer_ui_.gripper_state = "close"; } } void EngineerManual::sendCommand(const ros::Time& time) @@ -281,6 +284,7 @@ void EngineerManual::enterServo() { servo_mode_ = SERVO; gimbal_mode_ = DIRECT; + engineer_ui_.control_mode = "SERVO"; changeSpeedMode(EXCHANGE); servo_reset_caller_->callService(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); @@ -291,6 +295,7 @@ void EngineerManual::initMode() { servo_mode_ = JOINT; gimbal_mode_ = DIRECT; + engineer_ui_.control_mode = "JOINT"; changeSpeedMode(NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; @@ -483,14 +488,6 @@ void EngineerManual::ctrlSPress() void EngineerManual::ctrlVPress() { - if (gripper_state_ == "close") - { - runStepQueue("OPEN_GRIPPER"); - } - else - { - runStepQueue("CLOSE_GRIPPER"); - } } void EngineerManual::ctrlVRelease() { @@ -567,6 +564,7 @@ void EngineerManual::rPress() stone_num_++; else stone_num_ = 0; + engineer_ui_.stone_num = stone_num_; ROS_INFO_STREAM("Stone num is: " << stone_num_); } @@ -659,6 +657,16 @@ void EngineerManual::shiftRRelease() void EngineerManual::shiftVPress() { + if (gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + engineer_ui_.gripper_state = "OPEN"; + } + else + { + runStepQueue("CLOSE_GRIPPER"); + engineer_ui_.gripper_state = "CLOSE"; + } } void EngineerManual::shiftVRelease() { From 660297a343da326dca41b41d6b49dc8bc18a9a96 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 14 Apr 2024 15:53:59 +0800 Subject: [PATCH 07/14] Add f key to control gimbal height.Adjust key map.Remove ui stuff. --- include/rm_manual/engineer_manual.h | 2 +- src/engineer_manual.cpp | 27 +++++++++++++++++++-------- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 116120b0..17e4c589 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -141,7 +141,7 @@ class EngineerManual : public ChassisGimbalManual exchange_gyro_scale_{}, fast_speed_scale_{}, low_speed_scale_{}, normal_speed_scale_{}, exchange_speed_scale_{}; std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }; - int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }, gimbal_height_{ 0 }; ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index a1f9b95a..17daeeaa 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -209,7 +209,6 @@ void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) stone_num_--; else if (data->data == "+1" && stone_num_ < 2) stone_num_++; - engineer_ui_.stone_num = stone_num_; } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -217,12 +216,10 @@ void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) if (gpio_state_.gpio_state[0]) { gripper_state_ = "open"; - engineer_ui_.gripper_state = "open"; } else { gripper_state_ = "close"; - engineer_ui_.gripper_state = "close"; } } void EngineerManual::sendCommand(const ros::Time& time) @@ -284,7 +281,6 @@ void EngineerManual::enterServo() { servo_mode_ = SERVO; gimbal_mode_ = DIRECT; - engineer_ui_.control_mode = "SERVO"; changeSpeedMode(EXCHANGE); servo_reset_caller_->callService(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); @@ -295,7 +291,6 @@ void EngineerManual::initMode() { servo_mode_ = JOINT; gimbal_mode_ = DIRECT; - engineer_ui_.control_mode = "JOINT"; changeSpeedMode(NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; @@ -495,6 +490,10 @@ void EngineerManual::ctrlVRelease() void EngineerManual::ctrlWPress() { + prefix_ = "SIDE_"; + root_ = "BIG_ISLAND"; + runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlXPress() @@ -535,6 +534,21 @@ void EngineerManual::eRelease() void EngineerManual::fPress() { + switch (gimbal_height_) + { + case 0: + runStepQueue("GIMBAL_DOWN"); + gimbal_height_ = 1; + break; + case 1: + runStepQueue("GIMBAL_MID"); + gimbal_height_ = 2; + break; + case 2: + runStepQueue("GIMBAL_TALL"); + gimbal_height_ = 0; + break; + } } void EngineerManual::fRelease() { @@ -564,7 +578,6 @@ void EngineerManual::rPress() stone_num_++; else stone_num_ = 0; - engineer_ui_.stone_num = stone_num_; ROS_INFO_STREAM("Stone num is: " << stone_num_); } @@ -660,12 +673,10 @@ void EngineerManual::shiftVPress() if (gripper_state_ == "close") { runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state = "OPEN"; } else { runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state = "CLOSE"; } } void EngineerManual::shiftVRelease() From b7273dbf7247bb05f192563a4b46719d2e960926 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 15 Apr 2024 20:53:26 +0800 Subject: [PATCH 08/14] Switch exposure. --- include/rm_manual/chassis_gimbal_shooter_cover_manual.h | 1 + src/chassis_gimbal_shooter_cover_manual.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 577f292a..c7474c5a 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -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_; diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index cd3a402e..0006949c 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -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); @@ -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() @@ -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(); } } From 524bae3d31001dd17abc459bd4ccd2189c58bb2c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 15 Apr 2024 21:35:26 +0800 Subject: [PATCH 09/14] Add allow_shoot mode when switching armor target. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 1 + include/rm_manual/manual_base.h | 6 +++++- src/chassis_gimbal_shooter_manual.cpp | 6 ++++++ src/manual_base.cpp | 3 +++ 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 86132895..63c1491c 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -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 allowShootCallback(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); diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 32edca44..0a3dc82d 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,9 @@ class ManualBase virtual void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data) { } + virtual void allowShootCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) + { + } virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data) { } @@ -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_, suggest_fire_near_switching_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 4d22edd8..f7dfe683 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -139,6 +139,12 @@ void ChassisGimbalShooterManual::gimbalDesErrorCallback(const rm_msgs::GimbalDes shooter_cmd_sender_->updateGimbalDesError(*data); } +void ChassisGimbalShooterManual::allowShootCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) +{ + ChassisGimbalManual::allowShootCallback(data); + shooter_cmd_sender_->updateAllowShoot(*data); +} + void ChassisGimbalShooterManual::trackCallback(const rm_msgs::TrackData::ConstPtr& data) { ChassisGimbalManual::trackCallback(data); diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 87cb4482..33052090 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -31,6 +31,9 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); + suggest_fire_near_switching_sub_ = nh.subscribe( + "/controllers/gimbal_controller/bullet_solver/allow_shoot", 10, &ManualBase::allowShootCallback, this); + // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From f39ad035844b335f06e07204979c6dbda7368067 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 19 Apr 2024 22:04:22 +0800 Subject: [PATCH 10/14] Modify function and variable name. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- include/rm_manual/manual_base.h | 2 +- src/chassis_gimbal_shooter_manual.cpp | 6 +++--- src/manual_base.cpp | 5 +++-- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 63c1491c..ad4c74fe 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -48,7 +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 allowShootCallback(const rm_msgs::ShootBeforehandCmd ::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); diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 0a3dc82d..0d8529e1 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -72,7 +72,7 @@ class ManualBase virtual void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data) { } - virtual void allowShootCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) + virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { } virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index f7dfe683..d6ff8b36 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -139,10 +139,10 @@ void ChassisGimbalShooterManual::gimbalDesErrorCallback(const rm_msgs::GimbalDes shooter_cmd_sender_->updateGimbalDesError(*data); } -void ChassisGimbalShooterManual::allowShootCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) +void ChassisGimbalShooterManual::shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { - ChassisGimbalManual::allowShootCallback(data); - shooter_cmd_sender_->updateAllowShoot(*data); + ChassisGimbalManual::shootBeforehandCmdCallback(data); + shooter_cmd_sender_->updateShootBeforehandCmd(*data); } void ChassisGimbalShooterManual::trackCallback(const rm_msgs::TrackData::ConstPtr& data) diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 33052090..d772ad87 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -31,8 +31,9 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); - suggest_fire_near_switching_sub_ = nh.subscribe( - "/controllers/gimbal_controller/bullet_solver/allow_shoot", 10, &ManualBase::allowShootCallback, this); + suggest_fire_near_switching_sub_ = + nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, + &ManualBase::shootBeforehandCmdCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From 32c49aa65f1efaea46f028b471c8a301aca77b10 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 27 Apr 2024 23:01:02 +0800 Subject: [PATCH 11/14] Modify param name. --- include/rm_manual/manual_base.h | 2 +- src/manual_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 0d8529e1..861b9e9d 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -141,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_near_switching_sub_; + suggest_fire_sub_, shoot_beforehand_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/manual_base.cpp b/src/manual_base.cpp index d772ad87..7179c199 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -31,7 +31,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); - suggest_fire_near_switching_sub_ = + shoot_beforehand_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, &ManualBase::shootBeforehandCmdCallback, this); From 7b6ace14f45c200b9e07acda56223be3ebbb2e3f Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Wed, 1 May 2024 10:24:04 +0800 Subject: [PATCH 12/14] Change f_event_ to ctrl_f_event_. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 6 +++--- src/chassis_gimbal_shooter_manual.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 86132895..2af609e9 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -96,7 +96,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); } @@ -105,8 +105,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_{}; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 4d22edd8..e62b31e9 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -46,7 +46,6 @@ 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)); @@ -54,6 +53,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: 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)); @@ -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); From 4e5240d79085749fe2c7c31fa644049a31e1bbf0 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 3 May 2024 19:22:07 +0800 Subject: [PATCH 13/14] Modify param's name; --- include/rm_manual/manual_base.h | 2 +- src/manual_base.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 861b9e9d..bcbda8b5 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -141,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_, shoot_beforehand_sub_; + suggest_fire_sub_, shoot_beforehand_cmd_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 7179c199..f3db6d50 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -31,7 +31,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); - shoot_beforehand_sub_ = + shoot_beforehand_cmd_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, &ManualBase::shootBeforehandCmdCallback, this); From 34eee02a893a78ac6a10885ff25d7165aae1aa8b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 18:06:29 +0800 Subject: [PATCH 14/14] Update TrackData in vel_cmd_sender. --- src/chassis_gimbal_shooter_manual.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index d6ff8b36..70f4322e 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -149,6 +149,7 @@ void ChassisGimbalShooterManual::trackCallback(const rm_msgs::TrackData::ConstPt { ChassisGimbalManual::trackCallback(data); shooter_cmd_sender_->updateTrackData(*data); + vel_cmd_sender_->updateTrackData(*data); } void ChassisGimbalShooterManual::suggestFireCallback(const std_msgs::Bool::ConstPtr& data)