From 9267aa621e528b1cf10bd411a41583c702f5e171 Mon Sep 17 00:00:00 2001 From: cc0h <116955603+cc0h@users.noreply.github.com> Date: Mon, 30 Sep 2024 23:57:29 +0800 Subject: [PATCH] Revert "Merge rm_manual of 2024 engineer and engineer2." --- include/rm_manual/engineer2_manual.h | 193 ------ include/rm_manual/engineer_manual.h | 31 +- include/rm_manual/manual_base.h | 2 +- launch/load.launch | 2 +- src/chassis_gimbal_manual.cpp | 8 +- src/engineer2_manual.cpp | 844 --------------------------- src/engineer_manual.cpp | 393 ++----------- src/main.cpp | 3 - src/manual_base.cpp | 2 +- 9 files changed, 73 insertions(+), 1405 deletions(-) delete mode 100644 include/rm_manual/engineer2_manual.h delete mode 100644 src/engineer2_manual.cpp diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h deleted file mode 100644 index c7e4790e..00000000 --- a/include/rm_manual/engineer2_manual.h +++ /dev/null @@ -1,193 +0,0 @@ -// -// Created by qiayuan on 5/23/21. -// - -#pragma once - -#include "chassis_gimbal_manual.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "unordered_map" - -namespace rm_manual -{ -class Engineer2Manual : public ChassisGimbalManual -{ -public: - enum ControlMode - { - MANUAL, - MIDDLEWARE - }; - - enum JointMode - { - SERVO, - JOINT - }; - - enum GimbalMode - { - RATE, - DIRECT - }; - - enum SpeedMode - { - LOW, - NORMAL, - FAST, - EXCHANGE, - BIG_ISLAND_SPEED - }; - - Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); - void run() override; - -private: - void changeSpeedMode(SpeedMode speed_mode); - 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 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 initMode(); - void enterServo(); - void actionActiveCallback() - { - operating_mode_ = MIDDLEWARE; - } - void remoteControlTurnOff() override; - void chassisOutputOn() override; - void gimbalOutputOn() override; - - void rightSwitchUpRise() override; - void rightSwitchMidRise() override; - void rightSwitchDownRise() override; - void leftSwitchUpRise() override; - void leftSwitchUpFall(); - void leftSwitchDownRise() override; - void leftSwitchDownFall(); - void ctrlAPress(); - void ctrlBPress(); - void ctrlBPressing(); - void ctrlBRelease(); - void ctrlCPress(); - void ctrlDPress(); - 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 rRelease(); - void vPressing(); - void vRelease(); - void xPress(); - void zPressing(); - void zRelease(); - - void shiftPressing(); - void shiftRelease(); - void shiftBPress(); - void shiftBRelease(); - void shiftCPress(); - void shiftEPress(); - void shiftFPress(); - void shiftGPress(); - void shiftQPress(); - void shiftRPress(); - void shiftRRelease(); - void shiftVPress(); - void shiftVRelease(); - void shiftXPress(); - void shiftZPress(); - void shiftZRelease(); - - void mouseLeftRelease(); - void mouseRightRelease(); - - // Servo - - bool mouse_left_pressed_{}, mouse_right_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, - had_side_gold_{ false }, stone_state_[4]{}; - 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_{}, - big_island_speed_scale_{}; - - std::string prefix_{}, root_{}, exchange_direction_{ "left" }, exchange_arm_position_{ "normal" }; - int operating_mode_{}, servo_mode_{ 1 }, gimbal_mode_{}, gimbal_direction_{ 0 }; - - std::stack stone_num_{}; - - ros::Time last_time_; - ros::Subscriber stone_num_sub_, gripper_state_sub_; - ros::Publisher engineer_ui_pub_, gripper_ui_pub_; - - rm_msgs::GpioData gpio_state_; - rm_msgs::EngineerUi engineer_ui_, old_ui_; - rm_msgs::VisualizeStateData gripper_ui_; - - rm_common::Vel3DCommandSender* servo_command_sender_; - rm_common::ServiceCallerBase* servo_reset_caller_; - rm_common::CalibrationQueue* calibration_gather_{}; - - 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_; - - std::unordered_map stoneNumMap_ = { - { "+g", 0 }, { "+s1", 1 }, { "+s2", 2 }, { "+s3", 3 }, { "-g", 0 }, { "-s1", 1 }, { "-s2", 2 }, { "-s3", 3 }, - }; - - enum UiState - { - NONE, - BIG_ISLAND, - SMALL_ISLAND - }; -}; - -} // namespace rm_manual diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 1c2dff81..17e4c589 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -16,7 +16,6 @@ #include #include #include -#include namespace rm_manual { @@ -49,13 +48,6 @@ class EngineerManual : public ChassisGimbalManual EXCHANGE }; - enum ServoOrientation - { - MID, - RIGHT, - LEFT - }; - EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); void run() override; @@ -130,47 +122,40 @@ class EngineerManual : public ChassisGimbalManual void shiftBPress(); void shiftBRelease(); void shiftCPress(); - void shiftEPress(); void shiftFPress(); void shiftGPress(); - void shiftQPress(); void shiftRPress(); void shiftRRelease(); void shiftVPress(); void shiftVRelease(); void shiftXPress(); void shiftZPress(); - void shiftZRelease(); void mouseLeftRelease(); void mouseRightRelease(); // Servo - bool change_flag_{}, ore_rotator_pos_{ false }, shift_z_pressed_{ false }, ore_lifter_on_{ false }, - v_pressed_{ false }; - + bool change_flag_{}; double angular_z_scale_{}, gyro_scale_{}, fast_gyro_scale_{}, low_gyro_scale_{}, normal_gyro_scale_{}, exchange_gyro_scale_{}, fast_speed_scale_{}, low_speed_scale_{}, normal_speed_scale_{}, exchange_speed_scale_{}; - std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }, last_ore_{}; - - int operating_mode_{}, servo_mode_{}, servo_orientation_{ 0 }, gimbal_mode_{}, gimbal_height_{ 0 }, - gimbal_direction_{ 0 }, ore_lifter_pos_{ 0 }; - - std::stack stone_num_{}; + std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }, gimbal_height_{ 0 }; ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; ros::Publisher engineer_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::EngineerUi engineer_ui_, old_ui_; + rm_msgs::EngineerUi engineer_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; - - rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}; + 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/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index b8865c17..2ade4707 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -159,7 +159,7 @@ class ManualBase ros::NodeHandle nh_; ros::Time referee_last_get_stamp_; - bool remote_is_open_{ false }, referee_is_online_ = false; + bool remote_is_open_{}, referee_is_online_ = false; int state_ = PASSIVE; int robot_id_, chassis_power_; int chassis_output_on_ = 0, gimbal_output_on_ = 0, shooter_output_on_ = 0; diff --git a/launch/load.launch b/launch/load.launch index 19fa01f9..c6bd7512 100755 --- a/launch/load.launch +++ b/launch/load.launch @@ -1,5 +1,5 @@ - + diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index 0963033d..efd171ec 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -72,10 +72,10 @@ void ChassisGimbalManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_ ManualBase::checkKeyboard(dbus_data); if (robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER) { - w_event_.update((!dbus_data->key_ctrl) && dbus_data->key_w); - s_event_.update((!dbus_data->key_ctrl) && dbus_data->key_s); - a_event_.update((!dbus_data->key_ctrl) && dbus_data->key_a); - d_event_.update((!dbus_data->key_ctrl) && dbus_data->key_d); + w_event_.update((!dbus_data->key_ctrl) && (!dbus_data->key_shift) && dbus_data->key_w); + s_event_.update((!dbus_data->key_ctrl) && (!dbus_data->key_shift) && dbus_data->key_s); + a_event_.update((!dbus_data->key_ctrl) && (!dbus_data->key_shift) && dbus_data->key_a); + d_event_.update((!dbus_data->key_ctrl) && (!dbus_data->key_shift) && dbus_data->key_d); } else { diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp deleted file mode 100644 index 357eebe2..00000000 --- a/src/engineer2_manual.cpp +++ /dev/null @@ -1,844 +0,0 @@ -// -// Created by cch on 24-5-31. -// -#include "rm_manual/engineer2_manual.h" - -namespace rm_manual -{ -Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) - : ChassisGimbalManual(nh, nh_referee) - , operating_mode_(MANUAL) - , action_client_("/engineer_middleware/move_steps", true) -{ - engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); - ROS_INFO("Waiting for middleware to start."); - action_client_.waitForServer(); - ROS_INFO("Middleware started."); - stone_num_sub_ = nh.subscribe("/stone_num", 10, &Engineer2Manual::stoneNumCallback, this); - gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, - &Engineer2Manual::gpioStateCallback, this); - gripper_ui_pub_ = nh.advertise("/visualize_state", 10); - // 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"); - 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.1); - chassis_nh.param("exchange_speed_scale", exchange_speed_scale_, 0.2); - chassis_nh.param("big_island_speed_scale", big_island_speed_scale_, 0.02); - 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); - // Calibration - XmlRpc::XmlRpcValue rpc_value; - nh.getParam("calibration_gather", rpc_value); - calibration_gather_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); - left_switch_up_event_.setFalling(boost::bind(&Engineer2Manual::leftSwitchUpFall, this)); - left_switch_up_event_.setRising(boost::bind(&Engineer2Manual::leftSwitchUpRise, this)); - left_switch_down_event_.setFalling(boost::bind(&Engineer2Manual::leftSwitchDownFall, this)); - left_switch_down_event_.setRising(boost::bind(&Engineer2Manual::leftSwitchDownRise, this)); - ctrl_a_event_.setRising(boost::bind(&Engineer2Manual::ctrlAPress, this)); - ctrl_b_event_.setRising(boost::bind(&Engineer2Manual::ctrlBPress, this)); - ctrl_c_event_.setRising(boost::bind(&Engineer2Manual::ctrlCPress, this)); - ctrl_b_event_.setActiveHigh(boost::bind(&Engineer2Manual::ctrlBPressing, this)); - ctrl_b_event_.setFalling(boost::bind(&Engineer2Manual::ctrlBRelease, this)); - ctrl_d_event_.setRising(boost::bind(&Engineer2Manual::ctrlDPress, this)); - ctrl_e_event_.setRising(boost::bind(&Engineer2Manual::ctrlEPress, this)); - ctrl_f_event_.setRising(boost::bind(&Engineer2Manual::ctrlFPress, this)); - ctrl_g_event_.setRising(boost::bind(&Engineer2Manual::ctrlGPress, this)); - ctrl_q_event_.setRising(boost::bind(&Engineer2Manual::ctrlQPress, this)); - ctrl_r_event_.setRising(boost::bind(&Engineer2Manual::ctrlRPress, this)); - ctrl_s_event_.setRising(boost::bind(&Engineer2Manual::ctrlSPress, this)); - ctrl_v_event_.setRising(boost::bind(&Engineer2Manual::ctrlVPress, this)); - ctrl_v_event_.setFalling(boost::bind(&Engineer2Manual::ctrlVRelease, this)); - ctrl_w_event_.setRising(boost::bind(&Engineer2Manual::ctrlWPress, this)); - ctrl_x_event_.setRising(boost::bind(&Engineer2Manual::ctrlXPress, this)); - ctrl_z_event_.setRising(boost::bind(&Engineer2Manual::ctrlZPress, this)); - b_event_.setActiveHigh(boost::bind(&Engineer2Manual::bPressing, this)); - b_event_.setFalling(boost::bind(&Engineer2Manual::bRelease, this)); - c_event_.setActiveHigh(boost::bind(&Engineer2Manual::cPressing, this)); - c_event_.setFalling(boost::bind(&Engineer2Manual::cRelease, this)); - e_event_.setActiveHigh(boost::bind(&Engineer2Manual::ePressing, this)); - e_event_.setFalling(boost::bind(&Engineer2Manual::eRelease, this)); - f_event_.setRising(boost::bind(&Engineer2Manual::fPress, this)); - f_event_.setFalling(boost::bind(&Engineer2Manual::fRelease, this)); - g_event_.setRising(boost::bind(&Engineer2Manual::gPress, this)); - g_event_.setFalling(boost::bind(&Engineer2Manual::gRelease, this)); - q_event_.setActiveHigh(boost::bind(&Engineer2Manual::qPressing, this)); - q_event_.setFalling(boost::bind(&Engineer2Manual::qRelease, this)); - r_event_.setRising(boost::bind(&Engineer2Manual::rPress, this)); - r_event_.setFalling(boost::bind(&Engineer2Manual::rRelease, this)); - v_event_.setActiveHigh(boost::bind(&Engineer2Manual::vPressing, this)); - v_event_.setFalling(boost::bind(&Engineer2Manual::vRelease, this)); - x_event_.setRising(boost::bind(&Engineer2Manual::xPress, this)); - z_event_.setActiveHigh(boost::bind(&Engineer2Manual::zPressing, this)); - z_event_.setFalling(boost::bind(&Engineer2Manual::zRelease, this)); - shift_event_.setActiveHigh(boost::bind(&Engineer2Manual::shiftPressing, this)); - shift_event_.setFalling(boost::bind(&Engineer2Manual::shiftRelease, this)); - shift_b_event_.setRising(boost::bind(&Engineer2Manual::shiftBPress, this)); - shift_b_event_.setFalling(boost::bind(&Engineer2Manual::shiftBRelease, this)); - shift_c_event_.setRising(boost::bind(&Engineer2Manual::shiftCPress, this)); - shift_e_event_.setRising(boost::bind(&Engineer2Manual::shiftEPress, this)); - shift_f_event_.setRising(boost::bind(&Engineer2Manual::shiftFPress, this)); - shift_g_event_.setRising(boost::bind(&Engineer2Manual::shiftGPress, this)); - shift_q_event_.setRising(boost::bind(&Engineer2Manual::shiftQPress, this)); - shift_r_event_.setRising(boost::bind(&Engineer2Manual::shiftRPress, this)); - shift_r_event_.setFalling(boost::bind(&Engineer2Manual::shiftRRelease, this)); - shift_v_event_.setRising(boost::bind(&Engineer2Manual::shiftVPress, this)); - shift_v_event_.setFalling(boost::bind(&Engineer2Manual::shiftVRelease, this)); - shift_x_event_.setRising(boost::bind(&Engineer2Manual::shiftXPress, this)); - shift_z_event_.setRising(boost::bind(&Engineer2Manual::shiftZPress, this)); - shift_z_event_.setFalling(boost::bind(&Engineer2Manual::shiftZRelease, this)); - - mouse_left_event_.setFalling(boost::bind(&Engineer2Manual::mouseLeftRelease, this)); - mouse_right_event_.setFalling(boost::bind(&Engineer2Manual::mouseRightRelease, this)); -} - -void Engineer2Manual::run() -{ - ChassisGimbalManual::run(); - calibration_gather_->update(ros::Time::now()); - engineer_ui_pub_.publish(engineer_ui_); - gripper_ui_pub_.publish(gripper_ui_); -} - -void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) -{ - switch (speed_mode) - { - case LOW: - speed_change_scale_ = low_speed_scale_; - gyro_scale_ = low_gyro_scale_; - break; - case NORMAL: - speed_change_scale_ = normal_speed_scale_; - gyro_scale_ = normal_gyro_scale_; - break; - case FAST: - speed_change_scale_ = fast_speed_scale_; - gyro_scale_ = fast_gyro_scale_; - break; - case EXCHANGE: - speed_change_scale_ = exchange_speed_scale_; - gyro_scale_ = exchange_gyro_scale_; - break; - case BIG_ISLAND_SPEED: - speed_change_scale_ = big_island_speed_scale_; - gyro_scale_ = exchange_gyro_scale_; - default: - speed_change_scale_ = normal_speed_scale_; - gyro_scale_ = normal_gyro_scale_; - break; - } -} - -void Engineer2Manual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) -{ - ChassisGimbalManual::checkKeyboard(dbus_data); - ctrl_a_event_.update(dbus_data->key_ctrl & dbus_data->key_a); - 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_d_event_.update(dbus_data->key_ctrl & dbus_data->key_d); - ctrl_e_event_.update(dbus_data->key_ctrl & dbus_data->key_e); - 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_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); - - b_event_.update(dbus_data->key_b & !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); - 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_event_.update(dbus_data->key_shift & !dbus_data->key_ctrl); - shift_b_event_.update(dbus_data->key_shift & dbus_data->key_b); - 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_f_event_.update(dbus_data->key_shift & dbus_data->key_f); - 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_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); - - c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); -} - -void Engineer2Manual::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); - 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 Engineer2Manual::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); - chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; - if (servo_mode_ == JOINT) - vel_cmd_sender_->setAngularZVel(-dbus_data->m_x * gimbal_scale_); -} - -void Engineer2Manual::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 Engineer2Manual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) -{ - ManualBase::dbusDataCallback(data); - chassis_cmd_sender_->updateRefereeStatus(referee_is_online_); - if (servo_mode_ == SERVO) - updateServo(data); -} - -void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) -{ - auto it = stoneNumMap_.find(data->data); - if (it != stoneNumMap_.end()) - stone_state_[it->second] = (data->data[0] == '+'); -} - -void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) -{ - gripper_ui_.state = { data->gpio_state.begin(), data->gpio_state.end() - 2 }; - main_gripper_on_ = data->gpio_state[0]; -} - -void Engineer2Manual::sendCommand(const ros::Time& time) -{ - if (operating_mode_ == MANUAL) - { - chassis_cmd_sender_->sendChassisCommand(time, false); - vel_cmd_sender_->sendCommand(time); - } - if (servo_mode_ == SERVO) - { - changeSpeedMode(EXCHANGE); - servo_command_sender_->sendCommand(time); - if (gimbal_mode_ == RATE) - gimbal_cmd_sender_->sendCommand(time); - } -} - -void Engineer2Manual::runStepQueue(const std::string& step_queue_name) -{ - 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(&Engineer2Manual::actionDoneCallback, this, _1, _2), - boost::bind(&Engineer2Manual::actionActiveCallback, this), - boost::bind(&Engineer2Manual::actionFeedbackCallback, this, _1)); - operating_mode_ = MIDDLEWARE; - } - else - ROS_ERROR("Can not connect to middleware"); -} - -void Engineer2Manual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) -{ -} - -void Engineer2Manual::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()); - mouse_left_pressed_ = true; - mouse_right_pressed_ = true; - ROS_INFO("%i", result->finish); - operating_mode_ = MANUAL; - if (root_ == "HOME") - { - initMode(); - changeSpeedMode(NORMAL); - } - if (root_ == "BIG_ISLAND0" || root_ == "THREE_SILVER0") - engineer_ui_.symbol = UiState::NONE; - if (prefix_ == "LV4_" || prefix_ == "LV5_L_" || prefix_ == "LV5_R_") - { - had_ground_stone_ = false; - had_side_gold_ = false; - changeSpeedMode(EXCHANGE); - enterServo(); - } - if (root_ == "GROUND_STONE0") - { - changeSpeedMode(NORMAL); - } - if (prefix_ + root_ == "SIDE_BIG_ISLAND1") - { - enterServo(); - changeSpeedMode(BIG_ISLAND_SPEED); - } - if (prefix_ + root_ == "MID_BIG_ISLAND11" || prefix_ + root_ == "BOTH_BIG_ISLAND1") - { - enterServo(); - changeSpeedMode(BIG_ISLAND_SPEED); - } - if (prefix_ + root_ == "MID_BIG_ISLAND111" || prefix_ + root_ == "SIDE_BIG_ISLAND111" || - prefix_ + root_ == "BOTH_BIG_ISLAND111") - { - initMode(); - changeSpeedMode(NORMAL); - } -} - -void Engineer2Manual::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 = "yaw"; - engineer_ui_.control_mode = "SERVO"; -} - -void Engineer2Manual::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"; - engineer_ui_.control_mode = "NORMAL"; -} - -void Engineer2Manual::remoteControlTurnOff() -{ - ManualBase::remoteControlTurnOff(); - action_client_.cancelAllGoals(); -} - -void Engineer2Manual::gimbalOutputOn() -{ - ChassisGimbalManual::gimbalOutputOn(); -} - -void Engineer2Manual::chassisOutputOn() -{ - if (operating_mode_ == MIDDLEWARE) - action_client_.cancelAllGoals(); -} - -//-------------------controller input------------------- -void Engineer2Manual::rightSwitchUpRise() -{ - ChassisGimbalManual::rightSwitchUpRise(); - gimbal_mode_ = DIRECT; - servo_mode_ = JOINT; - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); -} -void Engineer2Manual::rightSwitchMidRise() -{ - ChassisGimbalManual::rightSwitchMidRise(); - servo_mode_ = JOINT; - gimbal_mode_ = RATE; - gimbal_cmd_sender_->setZero(); - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); -} -void Engineer2Manual::rightSwitchDownRise() -{ - 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 Engineer2Manual::leftSwitchUpRise() -{ - prefix_ = ""; - root_ = "CALIBRATION"; - runStepQueue("CALIBRATION"); - had_side_gold_ = false; - had_ground_stone_ = false; - calibration_gather_->reset(); - for (auto& stone : stone_state_) - { - stone = false; - } - engineer_ui_.control_mode = "NORMAL"; - ROS_INFO_STREAM("START CALIBRATE"); -} -void Engineer2Manual::leftSwitchUpFall() -{ - runStepQueue("HOME_WITH_PITCH"); -} - -void Engineer2Manual::leftSwitchDownRise() -{ - if (!main_gripper_on_) - { - runStepQueue("OA"); - } - else - { - runStepQueue("CA"); - } -} -void Engineer2Manual::leftSwitchDownFall() -{ - runStepQueue("MIDDLE_PITCH_UP"); -} - -//--------------------- keyboard input ------------------------ -// mouse input -void Engineer2Manual::mouseLeftRelease() -{ - if (mouse_left_pressed_) - { - root_ += "0"; - mouse_left_pressed_ = false; - runStepQueue(prefix_ + root_); - ROS_INFO("Finished %s", (prefix_ + root_).c_str()); - } -} - -void Engineer2Manual::mouseRightRelease() -{ - runStepQueue(prefix_ + root_); - ROS_INFO("Finished %s", (prefix_ + root_).c_str()); -} - -void Engineer2Manual::bPressing() -{ -} - -void Engineer2Manual::bRelease() -{ -} -void Engineer2Manual::cPressing() -{ - angular_z_scale_ = -0.8; -} -void Engineer2Manual::cRelease() -{ - angular_z_scale_ = 0.; -} -void Engineer2Manual::ePressing() -{ - if (servo_mode_ == SERVO) - vel_cmd_sender_->setAngularZVel(-gyro_scale_); -} -void Engineer2Manual::eRelease() -{ - if (servo_mode_ == SERVO) - vel_cmd_sender_->setAngularZVel(0.); -} -void Engineer2Manual::fPress() -{ -} -void Engineer2Manual::fRelease() -{ -} -void Engineer2Manual::gPress() -{ -} -void Engineer2Manual::gRelease() -{ -} -void Engineer2Manual::qPressing() -{ - if (servo_mode_ == SERVO) - vel_cmd_sender_->setAngularZVel(gyro_scale_); -} -void Engineer2Manual::qRelease() -{ - if (servo_mode_ == SERVO) - vel_cmd_sender_->setAngularZVel(0.); -} -void Engineer2Manual::rPress() -{ - if (had_side_gold_) - had_side_gold_ = false; - if (had_ground_stone_) - had_ground_stone_ = false; - else if (stone_state_[0]) - stone_state_[0] = false; - else if (stone_state_[3]) - stone_state_[3] = false; - else if (stone_state_[2]) - stone_state_[2] = false; - else if (stone_state_[1]) - stone_state_[1] = false; -} - -void Engineer2Manual::rRelease() -{ -} - -void Engineer2Manual::vPressing() -{ -} -void Engineer2Manual::vRelease() -{ -} -void Engineer2Manual::xPress() -{ - if (servo_mode_ == SERVO) - { - switch (gimbal_direction_) - { - case 0: - runStepQueue("GIMBAL_EE"); - gimbal_direction_ = 1; - break; - case 1: - runStepQueue("GIMBAL_R"); - gimbal_direction_ = 0; - break; - } - } - else - { - switch (gimbal_direction_) - { - case 0: - runStepQueue("GIMBAL_R"); - gimbal_direction_ = 1; - break; - case 1: - runStepQueue("GIMBAL_B"); - gimbal_direction_ = 2; - break; - case 2: - runStepQueue("GIMBAL_F"); - gimbal_direction_ = 0; - break; - } - } -} -void Engineer2Manual::zPressing() -{ - angular_z_scale_ = 0.8; -} -void Engineer2Manual::zRelease() -{ - angular_z_scale_ = 0.; -} - -//--------------------- CTRL --------------------- -void Engineer2Manual::ctrlAPress() -{ - prefix_ = ""; - root_ = "GET_SMALL_ISLAND"; - runStepQueue(prefix_ + root_); - changeSpeedMode(LOW); -} -void Engineer2Manual::ctrlBPress() -{ - prefix_ = ""; - root_ = "HOME"; - runStepQueue(prefix_ + root_); - changeSpeedMode(NORMAL); -} -void Engineer2Manual::ctrlBPressing() -{ -} -void Engineer2Manual::ctrlBRelease() -{ -} -void Engineer2Manual::ctrlCPress() -{ - action_client_.cancelAllGoals(); - changeSpeedMode(NORMAL); - initMode(); - ROS_INFO("cancel all goal"); -} -void Engineer2Manual::ctrlDPress() -{ - engineer_ui_.symbol = UiState::SMALL_ISLAND; - prefix_ = "GRIPPER_"; - root_ = "THREE_SILVER"; - changeSpeedMode(EXCHANGE); - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::ctrlEPress() -{ -} -void Engineer2Manual::ctrlFPress() -{ - if (exchange_direction_ == "left") - prefix_ = "LV5_L_"; - else - prefix_ = "LV5_R_"; - if (exchange_arm_position_ == "normal") - root_ = "EXCHANGE"; - else - root_ = "DROP_GOLD_EXCHANGE"; - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::ctrlGPress() -{ - engineer_ui_.symbol = UiState::BIG_ISLAND; - prefix_ = "MID_"; - root_ = "BIG_ISLAND"; - changeSpeedMode(LOW); - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::ctrlQPress() -{ -} -void Engineer2Manual::ctrlRPress() -{ - runStepQueue("CALIBRATION"); - prefix_ = ""; - root_ = "CALIBRATION"; - servo_mode_ = JOINT; - had_ground_stone_ = false; - had_side_gold_ = false; - calibration_gather_->reset(); - ROS_INFO_STREAM("START CALIBRATE"); - changeSpeedMode(NORMAL); - for (auto& stone : stone_state_) - stone = false; - runStepQueue("CA"); -} -void Engineer2Manual::ctrlSPress() -{ - engineer_ui_.symbol = UiState::BIG_ISLAND; - prefix_ = "BOTH_"; - root_ = "BIG_ISLAND"; - had_side_gold_ = true; - had_ground_stone_ = false; - changeSpeedMode(LOW); - runStepQueue(prefix_ + root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); -} -void Engineer2Manual::ctrlVPress() -{ -} -void Engineer2Manual::ctrlVRelease() -{ -} -void Engineer2Manual::ctrlWPress() -{ - engineer_ui_.symbol = UiState::BIG_ISLAND; - prefix_ = "SIDE_"; - root_ = "BIG_ISLAND"; - had_side_gold_ = true; - had_ground_stone_ = false; - changeSpeedMode(LOW); - runStepQueue(prefix_ + root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); -} -void Engineer2Manual::ctrlXPress() -{ - had_ground_stone_ = true; - prefix_ = ""; - root_ = "GROUND_STONE"; - changeSpeedMode(LOW); - ROS_INFO_STREAM(prefix_ + root_); - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::ctrlZPress() -{ -} - -//--------------- SHIFT -------------------------- - -void Engineer2Manual::shiftPressing() -{ - changeSpeedMode(FAST); -} -void Engineer2Manual::shiftRelease() -{ - changeSpeedMode(NORMAL); -} -void Engineer2Manual::shiftBPress() -{ -} -void Engineer2Manual::shiftBRelease() -{ -} -void Engineer2Manual::shiftCPress() -{ - action_client_.cancelAllGoals(); - if (servo_mode_ == SERVO) - { - initMode(); - ROS_INFO("EXIT SERVO"); - } - else - { - enterServo(); - ROS_INFO("ENTER SERVO"); - } - ROS_INFO("cancel all goal"); -} -void Engineer2Manual::shiftEPress() -{ - exchange_direction_ = "right"; - prefix_ = "LV5_R_"; - if (had_ground_stone_) - { - exchange_arm_position_ = "normal"; - root_ = "EXCHANGE"; - } - else if (had_side_gold_) - { - exchange_arm_position_ = "front"; - root_ = "DROP_GOLD_EXCHANGE"; - } - else - { - exchange_arm_position_ = "normal"; - if (stone_state_[0]) - root_ = "G"; - else if (stone_state_[3]) - root_ = "S3"; - else if (stone_state_[2]) - root_ = "S2"; - else if (stone_state_[1]) - root_ = "S1"; - } - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::shiftFPress() -{ - if (exchange_direction_ == "left") - { - prefix_ = "L2R_"; - exchange_direction_ = "right"; - } - else if (exchange_direction_ == "right") - { - prefix_ = "R2L_"; - exchange_direction_ = "left"; - } - if (exchange_arm_position_ == "normal") - root_ = "EXCHANGE"; - else - root_ = "EXCHANGE"; - ROS_INFO_STREAM(prefix_ + root_); - runStepQueue(prefix_ + root_); -} -void Engineer2Manual::shiftGPress() -{ - prefix_ = "LV4_"; - if (had_ground_stone_) - { - exchange_arm_position_ = "normal"; - root_ = "EXCHANGE"; - } - else if (had_side_gold_) - { - exchange_arm_position_ = "front"; - root_ = "EXCHANGE"; - } - else - { - exchange_arm_position_ = "normal"; - if (stone_state_[0]) - root_ = "G"; - else if (stone_state_[3]) - root_ = "S3"; - else if (stone_state_[2]) - root_ = "S2"; - else if (stone_state_[1]) - root_ = "S1"; - } - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM(prefix_ + root_); -} -void Engineer2Manual::shiftQPress() -{ - exchange_direction_ = "left"; - prefix_ = "LV5_L_"; - if (had_ground_stone_) - { - exchange_arm_position_ = "normal"; - root_ = "EXCHANGE"; - } - else if (had_side_gold_) - { - exchange_arm_position_ = "front"; - root_ = "DROP_GOLD_EXCHANGE"; - } - else - { - exchange_arm_position_ = "normal"; - if (stone_state_[0]) - root_ = "G"; - else if (stone_state_[3]) - root_ = "S3"; - else if (stone_state_[2]) - root_ = "S2"; - else if (stone_state_[1]) - root_ = "S1"; - } - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM(prefix_ + root_); -} -void Engineer2Manual::shiftRPress() -{ -} -void Engineer2Manual::shiftRRelease() -{ -} -void Engineer2Manual::shiftVPress() -{ - prefix_ = ""; - if (main_gripper_on_) - { - root_ = "CM"; - } - else - { - root_ = "OM"; - } - runStepQueue(root_); -} -void Engineer2Manual::shiftVRelease() -{ -} -void Engineer2Manual::shiftXPress() -{ -} -void Engineer2Manual::shiftZPress() -{ - prefix_ = ""; - root_ = "CG"; - runStepQueue(prefix_ + root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); -} -void Engineer2Manual::shiftZRelease() -{ -} - -} // namespace rm_manual diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index c8cc9ead..17daeeaa 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -33,14 +33,14 @@ 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_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); 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_); - nh.getParam("ore_bin_lifter_calibration", rpc_value); - ore_bin_lifter_calibration_ = 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)); @@ -84,17 +84,14 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) shift_b_event_.setRising(boost::bind(&EngineerManual::shiftBPress, this)); shift_b_event_.setFalling(boost::bind(&EngineerManual::shiftBRelease, this)); shift_c_event_.setRising(boost::bind(&EngineerManual::shiftCPress, this)); - shift_e_event_.setRising(boost::bind(&EngineerManual::shiftEPress, this)); shift_f_event_.setRising(boost::bind(&EngineerManual::shiftFPress, this)); shift_g_event_.setRising(boost::bind(&EngineerManual::shiftGPress, this)); - shift_q_event_.setRising(boost::bind(&EngineerManual::shiftQPress, this)); shift_r_event_.setRising(boost::bind(&EngineerManual::shiftRPress, 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)); - shift_z_event_.setFalling(boost::bind(&EngineerManual::shiftZRelease, this)); mouse_left_event_.setFalling(boost::bind(&EngineerManual::mouseLeftRelease, this)); mouse_right_event_.setFalling(boost::bind(&EngineerManual::mouseRightRelease, this)); @@ -104,12 +101,7 @@ void EngineerManual::run() { ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); - ore_bin_lifter_calibration_->update(ros::Time::now()); - if (engineer_ui_ != old_ui_) - { - engineer_ui_pub_.publish(engineer_ui_); - old_ui_ = engineer_ui_; - } + engineer_ui_pub_.publish(engineer_ui_); } void EngineerManual::changeSpeedMode(SpeedMode speed_mode) { @@ -167,7 +159,6 @@ void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) shift_b_event_.update(dbus_data->key_shift & dbus_data->key_b); 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_f_event_.update(dbus_data->key_shift & dbus_data->key_f); 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); @@ -202,20 +193,8 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) } void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { - switch (servo_orientation_) - { - case MID: - 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); - break; - case LEFT: - servo_command_sender_->setLinearVel(-dbus_data->wheel, -dbus_data->ch_l_y, dbus_data->ch_l_x); - servo_command_sender_->setAngularVel(-angular_z_scale_, dbus_data->ch_r_x, -dbus_data->ch_r_y); - break; - case RIGHT: - servo_command_sender_->setLinearVel(-dbus_data->wheel, dbus_data->ch_l_y, -dbus_data->ch_l_x); - servo_command_sender_->setAngularVel(-angular_z_scale_, -dbus_data->ch_r_x, dbus_data->ch_r_y); - } + 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) { @@ -226,19 +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_.empty()) - { - stone_num_.pop(); - engineer_ui_.stone_num--; - } - else if (stone_num_.size() < 2) - { - if (data->data == "GOLD") - stone_num_.push("GOLD"); - else if (data->data == "SILVER") - stone_num_.push("SILVER"); - engineer_ui_.stone_num++; - } + if (data->data == "-1" && stone_num_ > 0) + stone_num_--; + else if (data->data == "+1" && stone_num_ < 2) + stone_num_++; } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -246,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 = "CLOSED"; } } void EngineerManual::sendCommand(const ros::Time& time) @@ -296,31 +264,16 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& change_flag_ = true; ROS_INFO("%i", result->finish); operating_mode_ = MANUAL; - - if (prefix_ + root_ == "0_TAKE_ONE_STONE_SMALL_ISLAND0" || prefix_ + root_ == "TWO_STONE_SMALL_ISLAND0" || - prefix_ + root_ == "ORE_LIFTER_DOWN") - { - ore_bin_lifter_calibration_->reset(); - } - if (prefix_ + root_ == "EXCHANGE_POS" || prefix_ + root_ == "GET_DOWN_STONE_LEFT" || - prefix_ + root_ == "GET_DOWN_STONE_RIGHT" || prefix_ + root_ == "GET_UP_STONE_RIGHT" || - prefix_ + root_ == "GET_UP_STONE_LEFT" || prefix_ + root_ == "GET_DOWN_STONE_BIN" || - prefix_ + root_ == "GET_UP_STONE_BIN" || prefix_ + root_ == "EXCHANGE_L" || prefix_ + root_ == "EXCHANGE_R") + if (prefix_ + root_ == "EXCHANGE_POS" || prefix_ + root_ == "GET_DOWN_STONE_BIN" || + prefix_ + root_ == "GET_UP_STONE_BIN") { enterServo(); changeSpeedMode(EXCHANGE); - ore_bin_lifter_calibration_->reset(); } - if (root_ == "HOME") + if (prefix_ == "HOME_") { initMode(); changeSpeedMode(NORMAL); - ore_bin_lifter_calibration_->reset(); - } - if (root_ + prefix_ == "TWO_STONE_SMALL_ISLAND0" || root_ + prefix_ == "THREE_STONE_SMALL_ISLAND0" || - root_ + prefix_ == "MID_BIG_ISLAND0" || root_ + prefix_ == "SIDE_BIG_ISLAND0") - { - changeSpeedMode(NORMAL); } } @@ -333,7 +286,6 @@ void EngineerManual::enterServo() chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); action_client_.cancelAllGoals(); chassis_cmd_sender_->getMsg()->command_source_frame = "link4"; - engineer_ui_.control_mode = "SERVO"; } void EngineerManual::initMode() { @@ -342,7 +294,6 @@ void EngineerManual::initMode() changeSpeedMode(NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; - engineer_ui_.control_mode = "NORMAL"; } void EngineerManual::remoteControlTurnOff() @@ -350,7 +301,6 @@ void EngineerManual::remoteControlTurnOff() ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } - void EngineerManual::gimbalOutputOn() { ChassisGimbalManual::gimbalOutputOn(); @@ -395,17 +345,11 @@ void EngineerManual::leftSwitchUpRise() prefix_ = ""; root_ = "CALIBRATION"; calibration_gather_->reset(); - while (!stone_num_.empty()) - stone_num_.pop(); - ROS_INFO_STREAM("stone num is" << stone_num_.size()); - engineer_ui_.stone_num = 0; - engineer_ui_.gripper_state = "CLOSED"; - engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } void EngineerManual::leftSwitchUpFall() { - runStepQueue("HOME"); + runStepQueue("HOME_ZERO_STONE"); runStepQueue("CLOSE_GRIPPER"); } @@ -414,12 +358,10 @@ void EngineerManual::leftSwitchDownRise() if (gripper_state_ == "close") { runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state = "OPEN"; } else { runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state = "CLOSED"; } } void EngineerManual::leftSwitchDownFall() @@ -436,38 +378,40 @@ void EngineerManual::mouseLeftRelease() ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } } -//--------------------- keyboard input ------------------------ + void EngineerManual::mouseRightRelease() { runStepQueue(prefix_ + root_); ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } +//--------------------- keyboard input ------------------------ //------------------------- ctrl ------------------------------ void EngineerManual::ctrlAPress() { - if (stone_num_.size() == 0) - { - prefix_ = "0_TAKE_ONE_STONE_"; - } - else if (stone_num_.size() == 1) - { - prefix_ = "1_TAKE_ONE_STONE_"; - } + prefix_ = "ONE_STONE_"; root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void EngineerManual::ctrlBPress() { - prefix_ = ""; - root_ = "HOME"; - engineer_ui_.gripper_state = "CLOSED"; + prefix_ = "HOME_"; + switch (stone_num_) + { + case 0: + root_ = "ZERO_STONE"; + break; + case 1: + root_ = "ONE_STONE"; + break; + case 2: + root_ = "TWO_STONE"; + break; + } runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME"); - changeSpeedMode(NORMAL); + ROS_INFO_STREAM("RUN_HOME" << stone_num_); } void EngineerManual::ctrlCPress() @@ -482,7 +426,6 @@ void EngineerManual::ctrlDPress() root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void EngineerManual::ctrlEPress() @@ -491,47 +434,27 @@ void EngineerManual::ctrlEPress() void EngineerManual::ctrlFPress() { - if (root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") - { - prefix_ = ""; - root_ = "RIGHT_EXCHANGE"; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } - else if (root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") - { - prefix_ = ""; - root_ = "LEFT_EXCHANGE"; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } - else - { - prefix_ = ""; - root_ = "EXCHANGE_POS"; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } + prefix_ = ""; + root_ = "EXCHANGE_POS"; + runStepQueue(root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); } void EngineerManual::ctrlGPress() { - switch (stone_num_.size()) - { - case 0: - root_ = "STORE_WHEN_ZERO_STONE"; - stone_num_.push("SILVER"); - break; - case 1: - root_ = "DOWN_STONE_FROM_BIN"; - stone_num_.push("SILVER"); - 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"); } @@ -548,11 +471,6 @@ void EngineerManual::ctrlRPress() calibration_gather_->reset(); runStepQueue("CLOSE_GRIPPER"); ROS_INFO_STREAM("START CALIBRATE"); - changeSpeedMode(NORMAL); - while (!stone_num_.empty()) - stone_num_.pop(); - engineer_ui_.stone_num = 0; - ROS_INFO_STREAM("stone num is" << stone_num_.size()); } void EngineerManual::ctrlSPress() @@ -561,7 +479,6 @@ void EngineerManual::ctrlSPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void EngineerManual::ctrlVPress() @@ -577,7 +494,6 @@ void EngineerManual::ctrlWPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void EngineerManual::ctrlXPress() @@ -598,7 +514,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() { @@ -640,12 +556,6 @@ void EngineerManual::fRelease() void EngineerManual::gPress() { - if (stone_num_.size() < 2) - { - stone_num_.push("GOLD"); - engineer_ui_.stone_num = stone_num_.size(); - } - ROS_INFO_STREAM("Stone num is: " << stone_num_.size() << ", stone is " << stone_num_.top()); } void EngineerManual::gRelease() { @@ -664,59 +574,28 @@ void EngineerManual::qRelease() void EngineerManual::rPress() { - if (stone_num_.size() < 2) - { - stone_num_.push("SILVER"); - engineer_ui_.stone_num = stone_num_.size(); - } - ROS_INFO_STREAM("Stone num is: " << stone_num_.size() << ", stone is " << stone_num_.top()); + if (stone_num_ < 2) + stone_num_++; + else + stone_num_ = 0; + ROS_INFO_STREAM("Stone num is: " << stone_num_); } void EngineerManual::vPressing() { - if (!v_pressed_) - { - v_pressed_ = true; - if (!ore_rotator_pos_) - { - runStepQueue("ORE_ROTATOR_EXCHANGE"); - ore_rotator_pos_ = true; - } - else - { - runStepQueue("ORE_ROTATOR_INIT"); - ore_rotator_pos_ = false; - } - } } void EngineerManual::vRelease() { - v_pressed_ = false; } void EngineerManual::xPress() { - switch (gimbal_direction_) - { - case 0: - runStepQueue("GIMBAL_RIGHT"); - gimbal_direction_ = 1; - break; - case 1: - runStepQueue("GIMBAL_LEFT"); - gimbal_direction_ = 2; - break; - case 2: - runStepQueue("GIMBAL_FRONT"); - gimbal_direction_ = 0; - break; - } } void EngineerManual::zPressing() { 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::zRelease() { @@ -757,155 +636,23 @@ void EngineerManual::shiftCPress() ROS_INFO("cancel all goal"); } -void EngineerManual::shiftEPress() -{ - switch (stone_num_.size()) - { - case 0: - root_ = "NO STONE!!"; - break; - case 1: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_DOWN_STONE_RIGHT"; - servo_orientation_ = RIGHT; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_DOWN_STONE_RIGHT"; - servo_orientation_ = RIGHT; - ROS_INFO_STREAM("take but gold"); - } - break; - case 2: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_UP_STONE_RIGHT"; - servo_orientation_ = RIGHT; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_UP_STONE_RIGHT"; - servo_orientation_ = RIGHT; - ROS_INFO_STREAM("take but gold"); - } - break; - } - runStepQueue(root_); - prefix_ = ""; - - ROS_INFO("TAKE_STONE"); -} - void EngineerManual::shiftFPress() { - if (root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") - { - prefix_ = ""; - root_ = "EXCHANGE_POS"; - servo_orientation_ = MID; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } - else if (root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") - { - prefix_ = ""; - root_ = "RIGHT_EXCHANGE"; - servo_orientation_ = RIGHT; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } - else if (root_ == "GET_DOWN_STONE_BIN" || root_ == "GET_UP_STONE_BIN" || root_ == "EXCHANGE_POS") - { - prefix_ = ""; - root_ = "LEFT_EXCHANGE"; - servo_orientation_ = LEFT; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); - } } void EngineerManual::shiftGPress() { - switch (stone_num_.size()) + switch (stone_num_) { case 0: root_ = "NO STONE!!"; + stone_num_ = 0; break; case 1: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_DOWN_STONE_BIN"; - servo_orientation_ = MID; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_DOWN_STONE_BIN"; - servo_orientation_ = MID; - ROS_INFO_STREAM("take but gold"); - } + root_ = "GET_DOWN_STONE_BIN"; break; case 2: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_UP_STONE_BIN"; - servo_orientation_ = MID; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_UP_STONE_BIN"; - servo_orientation_ = MID; - ROS_INFO_STREAM("take but gold"); - } - break; - } - runStepQueue(root_); - prefix_ = ""; - - ROS_INFO("TAKE_STONE"); -} - -void EngineerManual::shiftQPress() -{ - switch (stone_num_.size()) - { - case 0: - root_ = "NO STONE!!"; - break; - case 1: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_DOWN_STONE_LEFT"; - servo_orientation_ = LEFT; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_DOWN_STONE_LEFT"; - servo_orientation_ = LEFT; - ROS_INFO_STREAM("take but gold"); - } - break; - case 2: - if (stone_num_.top() == "SILVER") - { - root_ = "GET_UP_STONE_LEFT"; - servo_orientation_ = LEFT; - ROS_INFO_STREAM("take but silver"); - } - else - { - root_ = "GET_UP_STONE_LEFT"; - servo_orientation_ = LEFT; - ROS_INFO_STREAM("take but gold"); - } + root_ = "GET_UP_STONE_BIN"; break; } runStepQueue(root_); @@ -926,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 = "CLOSED"; } } void EngineerManual::shiftVRelease() @@ -944,28 +689,6 @@ void EngineerManual::shiftXPress() void EngineerManual::shiftZPress() { - if (!shift_z_pressed_) - { - if (!ore_lifter_on_) - { - prefix_ = "ORE_LIFTER_UP"; - root_ = ""; - ore_lifter_on_ = true; - runStepQueue(prefix_ + root_); - } - else - { - prefix_ = "ORE_LIFTER_DOWN"; - root_ = ""; - ore_lifter_on_ = false; - runStepQueue(prefix_ + root_); - } - } -} - -void EngineerManual::shiftZRelease() -{ - shift_z_pressed_ = false; } } // namespace rm_manual diff --git a/src/main.cpp b/src/main.cpp index 8a10626e..b515ec08 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +5,6 @@ #include "rm_manual/manual_base.h" #include "rm_manual/chassis_gimbal_shooter_cover_manual.h" #include "rm_manual/engineer_manual.h" -#include "rm_manual/engineer2_manual.h" #include "rm_manual/dart_manual.h" #include "rm_manual/balance_manual.h" @@ -23,8 +22,6 @@ int main(int argc, char** argv) manual_control = new rm_manual::ChassisGimbalShooterManual(nh, nh_referee); else if (robot == "engineer") manual_control = new rm_manual::EngineerManual(nh, nh_referee); - else if (robot == "engineer2") - manual_control = new rm_manual::Engineer2Manual(nh, nh_referee); else if (robot == "dart") manual_control = new rm_manual::DartManual(nh, nh_referee); else if (robot == "balance") diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 3aace06e..d3b1da47 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -118,7 +118,7 @@ void ManualBase::actuatorStateCallback(const rm_msgs::ActuatorState::ConstPtr& d void ManualBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { - if (data->rc_is_open) + if (ros::Time::now() - data->stamp < ros::Duration(1.0)) { if (!remote_is_open_) {