diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h new file mode 100644 index 00000000..c7e4790e --- /dev/null +++ b/include/rm_manual/engineer2_manual.h @@ -0,0 +1,193 @@ +// +// 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 17e4c589..1c2dff81 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -16,6 +16,7 @@ #include #include #include +#include namespace rm_manual { @@ -48,6 +49,13 @@ class EngineerManual : public ChassisGimbalManual EXCHANGE }; + enum ServoOrientation + { + MID, + RIGHT, + LEFT + }; + EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); void run() override; @@ -122,40 +130,47 @@ 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_{}; + bool change_flag_{}, ore_rotator_pos_{ false }, shift_z_pressed_{ false }, ore_lifter_on_{ false }, + v_pressed_{ false }; + 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 }, gimbal_height_{ 0 }; + 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_{}; 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_; + rm_msgs::EngineerUi engineer_ui_, old_ui_; 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::CalibrationQueue *calibration_gather_{}, *pitch_calibration_; + + rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}; actionlib::SimpleActionClient action_client_; diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 2ade4707..b8865c17 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_{}, referee_is_online_ = false; + bool remote_is_open_{ false }, 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 c6bd7512..19fa01f9 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 efd171ec..0963033d 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_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); + 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); } else { diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp new file mode 100644 index 00000000..357eebe2 --- /dev/null +++ b/src/engineer2_manual.cpp @@ -0,0 +1,844 @@ +// +// 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 17daeeaa..c8cc9ead 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,14 +84,17 @@ 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)); @@ -101,7 +104,12 @@ void EngineerManual::run() { ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); - engineer_ui_pub_.publish(engineer_ui_); + ore_bin_lifter_calibration_->update(ros::Time::now()); + if (engineer_ui_ != old_ui_) + { + engineer_ui_pub_.publish(engineer_ui_); + old_ui_ = engineer_ui_; + } } void EngineerManual::changeSpeedMode(SpeedMode speed_mode) { @@ -159,6 +167,7 @@ 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); @@ -193,8 +202,20 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) } 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); + 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); + } } void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { @@ -205,10 +226,19 @@ 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) - stone_num_--; - else if (data->data == "+1" && stone_num_ < 2) - stone_num_++; + 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++; + } } void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { @@ -216,10 +246,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 = "CLOSED"; } } void EngineerManual::sendCommand(const ros::Time& time) @@ -264,16 +296,31 @@ 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") + + 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") { enterServo(); changeSpeedMode(EXCHANGE); + ore_bin_lifter_calibration_->reset(); } - if (prefix_ == "HOME_") + if (root_ == "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); } } @@ -286,6 +333,7 @@ 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() { @@ -294,6 +342,7 @@ 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() @@ -301,6 +350,7 @@ void EngineerManual::remoteControlTurnOff() ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } + void EngineerManual::gimbalOutputOn() { ChassisGimbalManual::gimbalOutputOn(); @@ -345,11 +395,17 @@ 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_ZERO_STONE"); + runStepQueue("HOME"); runStepQueue("CLOSE_GRIPPER"); } @@ -358,10 +414,12 @@ 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() @@ -378,40 +436,38 @@ 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() { - prefix_ = "ONE_STONE_"; + if (stone_num_.size() == 0) + { + prefix_ = "0_TAKE_ONE_STONE_"; + } + else if (stone_num_.size() == 1) + { + prefix_ = "1_TAKE_ONE_STONE_"; + } root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlBPress() { - prefix_ = "HOME_"; - switch (stone_num_) - { - case 0: - root_ = "ZERO_STONE"; - break; - case 1: - root_ = "ONE_STONE"; - break; - case 2: - root_ = "TWO_STONE"; - break; - } + prefix_ = ""; + root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME" << stone_num_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); } void EngineerManual::ctrlCPress() @@ -426,6 +482,7 @@ void EngineerManual::ctrlDPress() root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlEPress() @@ -434,27 +491,47 @@ void EngineerManual::ctrlEPress() void EngineerManual::ctrlFPress() { - prefix_ = ""; - root_ = "EXCHANGE_POS"; - runStepQueue(root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); + 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); + } } void EngineerManual::ctrlGPress() { - // 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_ = ""; + 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_ = ""; // ROS_INFO("STORE_STONE"); } @@ -471,6 +548,11 @@ 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() @@ -479,6 +561,7 @@ void EngineerManual::ctrlSPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlVPress() @@ -494,6 +577,7 @@ void EngineerManual::ctrlWPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlXPress() @@ -514,7 +598,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() { @@ -556,6 +640,12 @@ 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() { @@ -574,28 +664,59 @@ void EngineerManual::qRelease() void EngineerManual::rPress() { - if (stone_num_ < 2) - stone_num_++; - else - stone_num_ = 0; - ROS_INFO_STREAM("Stone num is: " << stone_num_); + 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()); } 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() { @@ -636,23 +757,155 @@ 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_) + switch (stone_num_.size()) { case 0: root_ = "NO STONE!!"; - stone_num_ = 0; break; case 1: - root_ = "GET_DOWN_STONE_BIN"; + 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"); + } break; case 2: - root_ = "GET_UP_STONE_BIN"; + 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"); + } break; } runStepQueue(root_); @@ -673,10 +926,12 @@ 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() @@ -689,6 +944,28 @@ 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 b515ec08..8a10626e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,7 @@ #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" @@ -22,6 +23,8 @@ 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 87f73004..db82ba89 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -8,11 +8,13 @@ namespace rm_manual ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : controller_manager_(nh), tf_listener_(tf_buffer_), nh_(nh) { + std::string dbus_topic_; + nh.getParam("dbus_topic", dbus_topic_); // sub joint_state_sub_ = nh.subscribe("/joint_states", 10, &ManualBase::jointStateCallback, this); actuator_state_sub_ = nh.subscribe("/actuator_states", 10, &ManualBase::actuatorStateCallback, this); - dbus_sub_ = nh.subscribe("/dbus_data", 10, &ManualBase::dbusDataCallback, this); + dbus_sub_ = nh.subscribe(dbus_topic_, 10, &ManualBase::dbusDataCallback, this); track_sub_ = nh.subscribe("/track", 10, &ManualBase::trackCallback, this); gimbal_des_error_sub_ = nh.subscribe("/controllers/gimbal_controller/error", 10, &ManualBase::gimbalDesErrorCallback, this); @@ -114,7 +116,7 @@ void ManualBase::actuatorStateCallback(const rm_msgs::ActuatorState::ConstPtr& d void ManualBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { - if (ros::Time::now() - data->stamp < ros::Duration(1.0)) + if (data->rc_is_open) { if (!remote_is_open_) {