From 16d42b6eae86ae068f1e5d0d38a2c1be08717b48 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 24 Apr 2024 01:03:06 +0800 Subject: [PATCH 01/42] Add gold ore and silver ore.Add ui stuff. --- include/rm_manual/engineer_manual.h | 7 +- src/engineer_manual.cpp | 114 ++++++++++++++++++++-------- 2 files changed, 89 insertions(+), 32 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 17e4c589..cc1be20a 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 { @@ -141,14 +142,16 @@ class EngineerManual : public ChassisGimbalManual exchange_gyro_scale_{}, fast_speed_scale_{}, low_speed_scale_{}, normal_speed_scale_{}, exchange_speed_scale_{}; std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }; - int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, stone_num_{ 0 }, gimbal_height_{ 0 }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_height_{ 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_; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 17daeeaa..7c3d2856 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -101,7 +101,11 @@ void EngineerManual::run() { ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); - engineer_ui_pub_.publish(engineer_ui_); + if (engineer_ui_ != old_ui_) + { + engineer_ui_pub_.publish(engineer_ui_); + old_ui_ = engineer_ui_; + } } void EngineerManual::changeSpeedMode(SpeedMode speed_mode) { @@ -205,10 +209,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 +229,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) @@ -270,11 +285,16 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& enterServo(); changeSpeedMode(EXCHANGE); } - if (prefix_ == "HOME_") + if (root_ == "HOME") { initMode(); changeSpeedMode(NORMAL); } + 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); + } } void EngineerManual::enterServo() @@ -286,6 +306,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 +315,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() @@ -345,11 +367,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 +386,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() @@ -397,21 +427,11 @@ void EngineerManual::ctrlAPress() 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"; runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME" << stone_num_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); } void EngineerManual::ctrlCPress() @@ -426,6 +446,7 @@ void EngineerManual::ctrlDPress() root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlEPress() @@ -438,6 +459,7 @@ void EngineerManual::ctrlFPress() root_ = "EXCHANGE_POS"; runStepQueue(root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(EXCHANGE); } void EngineerManual::ctrlGPress() @@ -471,6 +493,10 @@ void EngineerManual::ctrlRPress() calibration_gather_->reset(); runStepQueue("CLOSE_GRIPPER"); ROS_INFO_STREAM("START CALIBRATE"); + changeSpeedMode(NORMAL); + while (!stone_num_.empty()) + stone_num_.pop(); + ROS_INFO_STREAM("stone num is" << stone_num_.size()); } void EngineerManual::ctrlSPress() @@ -479,6 +505,7 @@ void EngineerManual::ctrlSPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlVPress() @@ -494,6 +521,7 @@ void EngineerManual::ctrlWPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlXPress() @@ -556,6 +584,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,11 +608,12 @@ 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() @@ -642,17 +677,34 @@ void EngineerManual::shiftFPress() 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"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_DOWN_STONE_BIN"; + ROS_INFO_STREAM("take but gold"); + } break; case 2: - root_ = "GET_UP_STONE_BIN"; + if (stone_num_.top() == "SILVER") + { + root_ = "GET_DOWN_STONE_BIN"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_DOWN_STONE_BIN"; + ROS_INFO_STREAM("take but gold"); + } break; } runStepQueue(root_); @@ -673,10 +725,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() From 175d07eaa4230442584d8a45cab3cebb88f8014a Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 26 Apr 2024 01:52:56 +0800 Subject: [PATCH 02/42] Fix no speed mode change in ctrl a.Fix servo command direction error. --- src/engineer_manual.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 7c3d2856..93ea5806 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -197,8 +197,8 @@ 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); + 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) { @@ -323,6 +323,7 @@ void EngineerManual::remoteControlTurnOff() ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } + void EngineerManual::gimbalOutputOn() { ChassisGimbalManual::gimbalOutputOn(); @@ -423,6 +424,7 @@ void EngineerManual::ctrlAPress() root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlBPress() From 3a5ce4d425be61edc4e73eb501ec07d76700d723 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Apr 2024 22:43:20 +0800 Subject: [PATCH 03/42] Fix bug,add ore stuck solve motion. --- include/rm_manual/engineer_manual.h | 2 +- src/engineer_manual.cpp | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index cc1be20a..6e8df30f 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -137,7 +137,7 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}; + bool change_flag_{}, ore_lifter_pos_{ 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_{}; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 93ea5806..03871ef9 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -699,12 +699,12 @@ void EngineerManual::shiftGPress() case 2: if (stone_num_.top() == "SILVER") { - root_ = "GET_DOWN_STONE_BIN"; + root_ = "GET_UP_STONE_BIN"; ROS_INFO_STREAM("take but silver"); } else { - root_ = "GET_DOWN_STONE_BIN"; + root_ = "GET_UP_STONE_BIN"; ROS_INFO_STREAM("take but gold"); } break; @@ -745,6 +745,16 @@ void EngineerManual::shiftXPress() void EngineerManual::shiftZPress() { + if (ore_lifter_pos_) + { + runStepQueue("ORE_LIFTER_MID"); + ore_lifter_pos_ = true; + } + else + { + runStepQueue("ORE_LIFTER_DOWN"); + ore_lifter_pos_ = false; + } } } // namespace rm_manual From 1748fcac89dc14432debe4c4f199f05798a3f144 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Apr 2024 01:33:33 +0800 Subject: [PATCH 04/42] change servo direction --- src/engineer_manual.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 03871ef9..700b8094 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -197,8 +197,8 @@ 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); + 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) { @@ -544,7 +544,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() { @@ -632,7 +632,7 @@ void EngineerManual::xPress() 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() { From 4d0196d65cb7982d030a2e0633f3569964e67020 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 1 May 2024 22:09:15 +0800 Subject: [PATCH 05/42] Add shift q and e to take ore from different direction. --- include/rm_manual/engineer_manual.h | 2 + src/engineer_manual.cpp | 83 ++++++++++++++++++++++++++++- 2 files changed, 83 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 6e8df30f..402d0ed6 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -123,8 +123,10 @@ class EngineerManual : public ChassisGimbalManual void shiftBPress(); void shiftBRelease(); void shiftCPress(); + void shiftEPress(); void shiftFPress(); void shiftGPress(); + void shiftQPress(); void shiftRPress(); void shiftRRelease(); void shiftVPress(); diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 700b8094..378cc6bf 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -84,8 +84,10 @@ 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)); @@ -279,8 +281,9 @@ 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_ == "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") { enterServo(); changeSpeedMode(EXCHANGE); @@ -673,6 +676,44 @@ 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"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_DOWN_STONE_RIGHT"; + ROS_INFO_STREAM("take but gold"); + } + break; + case 2: + if (stone_num_.top() == "SILVER") + { + root_ = "GET_UP_STONE_RIGHT"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_UP_STONE_RIGHT"; + ROS_INFO_STREAM("take but gold"); + } + break; + } + runStepQueue(root_); + prefix_ = ""; + + ROS_INFO("TAKE_STONE"); +} + void EngineerManual::shiftFPress() { } @@ -715,6 +756,44 @@ void EngineerManual::shiftGPress() 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"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_DOWN_STONE_LEFT"; + ROS_INFO_STREAM("take but gold"); + } + break; + case 2: + if (stone_num_.top() == "SILVER") + { + root_ = "GET_UP_STONE_LEFT"; + ROS_INFO_STREAM("take but silver"); + } + else + { + root_ = "GET_UP_STONE_LEFT"; + ROS_INFO_STREAM("take but gold"); + } + break; + } + runStepQueue(root_); + prefix_ = ""; + + ROS_INFO("TAKE_STONE"); +} + void EngineerManual::shiftRPress() { } From bcb3ff9050a9d2ec4449640eaf78d132f5d2b382 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 4 May 2024 01:58:42 +0800 Subject: [PATCH 06/42] fix ore lifter don't fall when an ore is in bin. --- src/engineer_manual.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 378cc6bf..22bad6c6 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -423,7 +423,14 @@ void EngineerManual::mouseRightRelease() //------------------------- 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()); From 3ed835f713ba0c9fec6ad31ef636748575eca153 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 7 May 2024 18:24:19 +0800 Subject: [PATCH 07/42] record engineer manual. --- include/rm_manual/engineer_manual.h | 6 ++-- src/engineer_manual.cpp | 53 ++++++++++++++++++++++------- 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 402d0ed6..3420f0d6 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -139,12 +139,12 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_lifter_pos_{ false }; + bool change_flag_{}, ore_lifter_pos_{ false }, ore_rotator_pos_{ 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_{}, gimbal_height_{ 0 }; + std::string prefix_{}, root_{}, reversal_state_{}, drag_state_{ "off" }, gripper_state_{ "off" }, last_ore_{}; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_height_{ 0 }, gimbal_direction_{ 0 }; std::stack stone_num_{}; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 22bad6c6..c62a3d4c 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -441,6 +441,7 @@ void EngineerManual::ctrlBPress() { prefix_ = ""; root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; runStepQueue(prefix_ + root_); ROS_INFO_STREAM("RUN_HOME"); changeSpeedMode(NORMAL); @@ -476,19 +477,19 @@ void EngineerManual::ctrlFPress() 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"); } @@ -508,6 +509,7 @@ void EngineerManual::ctrlRPress() changeSpeedMode(NORMAL); while (!stone_num_.empty()) stone_num_.pop(); + engineer_ui_.stone_num = 0; ROS_INFO_STREAM("stone num is" << stone_num_.size()); } @@ -630,6 +632,16 @@ void EngineerManual::rPress() void EngineerManual::vPressing() { + if (!ore_rotator_pos_) + { + runStepQueue("ORE_ROTATOR_EXCHANGE"); + ore_rotator_pos_ = true; + } + else + { + runStepQueue("ORE_ROTATOR_INIT"); + ore_rotator_pos_ = false; + } } void EngineerManual::vRelease() { @@ -637,6 +649,21 @@ void EngineerManual::vRelease() 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() From 6346c823ffad2e96ac7aa45e7c4984d7625a99c6 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 8 May 2024 01:50:40 +0800 Subject: [PATCH 08/42] modify. --- src/chassis_gimbal_manual.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index cd250d1c..bcd2e9f7 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -64,10 +64,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 { From 105bf11750027dfda8e9f9bd4b7100361b4b0fcd Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 8 May 2024 01:51:24 +0800 Subject: [PATCH 09/42] calibrate ore lifter once it goes to button. --- include/rm_manual/engineer_manual.h | 4 ++-- src/engineer_manual.cpp | 19 +++++++++++++++---- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 3420f0d6..f34ec3aa 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -139,7 +139,7 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_lifter_pos_{ false }, ore_rotator_pos_{ false }; + bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ 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_{}; @@ -160,7 +160,7 @@ class EngineerManual : public ChassisGimbalManual 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/src/engineer_manual.cpp b/src/engineer_manual.cpp index c62a3d4c..08fe48ed 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -41,6 +41,8 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) 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)); @@ -103,6 +105,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_); @@ -281,17 +284,25 @@ 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_UP_STONE_LEFT" || prefix_ + root_ == "GET_DOWN_STONE_BIN" || + prefix_ + root_ == "GET_UP_STONE_BIN") { enterServo(); changeSpeedMode(EXCHANGE); + ore_bin_lifter_calibration_->reset(); } 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") @@ -858,15 +869,15 @@ void EngineerManual::shiftXPress() void EngineerManual::shiftZPress() { - if (ore_lifter_pos_) + if (!ore_lifter_on_) { runStepQueue("ORE_LIFTER_MID"); - ore_lifter_pos_ = true; + ore_lifter_on_ = true; } else { runStepQueue("ORE_LIFTER_DOWN"); - ore_lifter_pos_ = false; + ore_lifter_on_ = false; } } From 68db9423ba0bbeaf0b3ea1e71f0c0026eb7fb1ea Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 8 May 2024 23:57:46 +0800 Subject: [PATCH 10/42] Add joint2 calibration before run HOME. --- include/rm_manual/engineer_manual.h | 6 +++-- src/engineer_manual.cpp | 41 ++++++++++++++++++++++++----- 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index f34ec3aa..f93a1e68 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -85,6 +85,7 @@ class EngineerManual : public ChassisGimbalManual void leftSwitchDownFall(); void ctrlAPress(); void ctrlBPress(); + void ctrlBPressing(); void ctrlCPress(); void ctrlDPress(); void ctrlEPress(); @@ -139,7 +140,7 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }; + bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }, joint2_calibrated_{ 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_{}; @@ -160,7 +161,8 @@ class EngineerManual : public ChassisGimbalManual 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_, *ore_bin_lifter_calibration_{}; + rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}, + *joint2_calibration_{}; actionlib::SimpleActionClient action_client_; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 08fe48ed..e700d19c 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -43,6 +43,8 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) 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_); + nh.getParam("joint2_calibration", rpc_value); + joint2_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)); @@ -51,6 +53,7 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ctrl_a_event_.setRising(boost::bind(&EngineerManual::ctrlAPress, this)); ctrl_b_event_.setRising(boost::bind(&EngineerManual::ctrlBPress, this)); ctrl_c_event_.setRising(boost::bind(&EngineerManual::ctrlCPress, this)); + ctrl_b_event_.setActiveHigh(boost::bind(&EngineerManual::ctrlBPressing, this)); ctrl_d_event_.setRising(boost::bind(&EngineerManual::ctrlDPress, this)); ctrl_e_event_.setRising(boost::bind(&EngineerManual::ctrlEPress, this)); ctrl_f_event_.setRising(boost::bind(&EngineerManual::ctrlFPress, this)); @@ -106,6 +109,7 @@ void EngineerManual::run() ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); ore_bin_lifter_calibration_->update(ros::Time::now()); + joint2_calibration_->update(ros::Time::now()); if (engineer_ui_ != old_ui_) { engineer_ui_pub_.publish(engineer_ui_); @@ -289,6 +293,11 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& { ore_bin_lifter_calibration_->reset(); } + if (prefix_ + root_ == "HOME") + { + joint2_calibrated_ = false; + ROS_INFO_STREAM("joint2_calibrated_ IS" << joint2_calibrated_); + } 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" || @@ -450,12 +459,32 @@ void EngineerManual::ctrlAPress() void EngineerManual::ctrlBPress() { - prefix_ = ""; - root_ = "HOME"; - engineer_ui_.gripper_state = "CLOSED"; - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME"); - changeSpeedMode(NORMAL); + // joint2_calibration_->reset(); + // prefix_ = ""; + // root_ = "HOME"; + // engineer_ui_.gripper_state = "CLOSED"; + // runStepQueue(prefix_ + root_); + // ROS_INFO_STREAM("RUN_HOME"); + // changeSpeedMode(NORMAL); + // +} + +void EngineerManual::ctrlBPressing() +{ + if (!joint2_calibrated_) + { + joint2_calibration_->reset(); + joint2_calibrated_ = true; + } + if (joint2_calibration_->isCalibrated()) + { + prefix_ = ""; + root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; + runStepQueue(prefix_ + root_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); + } } void EngineerManual::ctrlCPress() From 3d4622017391e3c4751b5bd4303572c9ebbf3df8 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 11 May 2024 17:54:54 +0800 Subject: [PATCH 11/42] pressing ctrl b to calibrate and run home once. --- include/rm_manual/engineer_manual.h | 4 +++- src/engineer_manual.cpp | 37 +++++++++++++++++++---------- 2 files changed, 27 insertions(+), 14 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index f93a1e68..0a8dff21 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -86,6 +86,7 @@ class EngineerManual : public ChassisGimbalManual void ctrlAPress(); void ctrlBPress(); void ctrlBPressing(); + void ctrlBRelease(); void ctrlCPress(); void ctrlDPress(); void ctrlEPress(); @@ -140,7 +141,8 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }, joint2_calibrated_{ false }; + bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, + joint2_homed_{ 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_{}; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index e700d19c..10ae5f00 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -54,6 +54,7 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ctrl_b_event_.setRising(boost::bind(&EngineerManual::ctrlBPress, this)); ctrl_c_event_.setRising(boost::bind(&EngineerManual::ctrlCPress, this)); ctrl_b_event_.setActiveHigh(boost::bind(&EngineerManual::ctrlBPressing, this)); + ctrl_b_event_.setFalling(boost::bind(&EngineerManual::ctrlBRelease, this)); ctrl_d_event_.setRising(boost::bind(&EngineerManual::ctrlDPress, this)); ctrl_e_event_.setRising(boost::bind(&EngineerManual::ctrlEPress, this)); ctrl_f_event_.setRising(boost::bind(&EngineerManual::ctrlFPress, this)); @@ -296,7 +297,7 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& if (prefix_ + root_ == "HOME") { joint2_calibrated_ = false; - ROS_INFO_STREAM("joint2_calibrated_ IS" << joint2_calibrated_); + joint2_homed_ = true; } if (prefix_ + root_ == "EXCHANGE_POS" || prefix_ + root_ == "GET_DOWN_STONE_LEFT" || prefix_ + root_ == "GET_DOWN_STONE_RIGHT" || prefix_ + root_ == "GET_UP_STONE_RIGHT" || @@ -471,22 +472,32 @@ void EngineerManual::ctrlBPress() void EngineerManual::ctrlBPressing() { - if (!joint2_calibrated_) + if (!joint2_homed_) { - joint2_calibration_->reset(); - joint2_calibrated_ = true; - } - if (joint2_calibration_->isCalibrated()) - { - prefix_ = ""; - root_ = "HOME"; - engineer_ui_.gripper_state = "CLOSED"; - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME"); - changeSpeedMode(NORMAL); + if (!joint2_calibrated_) + { + joint2_calibration_->reset(); + joint2_calibrated_ = true; + } + if (joint2_calibration_->isCalibrated()) + { + ROS_INFO_STREAM("joint2 homed is " << joint2_homed_); + prefix_ = ""; + root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; + runStepQueue(prefix_ + root_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); + } } } +void EngineerManual::ctrlBRelease() +{ + joint2_homed_ = false; + ROS_INFO_STREAM("joint2 homed is " << joint2_homed_); +} + void EngineerManual::ctrlCPress() { action_client_.cancelAllGoals(); From 822b71dcbf384ac3cf08b330ad5699ce75432d53 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sun, 12 May 2024 03:11:31 +0800 Subject: [PATCH 12/42] Fix the issue where remote_is_open_ is initialized but not assigned a value.. --- include/rm_manual/manual_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index bcbda8b5..26089c26 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -155,7 +155,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; From 2eca9f78cfc1499dab66cce195a46a1eb08a7776 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 13 May 2024 02:37:28 +0800 Subject: [PATCH 13/42] fix problem: press ctrl B twice to run home. --- include/rm_manual/engineer_manual.h | 2 +- src/engineer_manual.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 0a8dff21..6e544029 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -142,7 +142,7 @@ class EngineerManual : public ChassisGimbalManual // Servo bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, - joint2_homed_{ false }; + joint2_homed_{ false }, b_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_{}; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 10ae5f00..c50e81a1 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -297,7 +297,8 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& if (prefix_ + root_ == "HOME") { joint2_calibrated_ = false; - joint2_homed_ = true; + if (b_pressed_) + joint2_homed_ = true; } if (prefix_ + root_ == "EXCHANGE_POS" || prefix_ + root_ == "GET_DOWN_STONE_LEFT" || prefix_ + root_ == "GET_DOWN_STONE_RIGHT" || prefix_ + root_ == "GET_UP_STONE_RIGHT" || @@ -444,7 +445,7 @@ void EngineerManual::mouseRightRelease() //------------------------- ctrl ------------------------------ void EngineerManual::ctrlAPress() { - if (stone_num_.size() == 0) + if (stone_num_.empty()) { prefix_ = "0_TAKE_ONE_STONE_"; } @@ -472,8 +473,10 @@ void EngineerManual::ctrlBPress() void EngineerManual::ctrlBPressing() { + b_pressed_ = true; if (!joint2_homed_) { + initMode(); if (!joint2_calibrated_) { joint2_calibration_->reset(); @@ -481,12 +484,10 @@ void EngineerManual::ctrlBPressing() } if (joint2_calibration_->isCalibrated()) { - ROS_INFO_STREAM("joint2 homed is " << joint2_homed_); prefix_ = ""; root_ = "HOME"; engineer_ui_.gripper_state = "CLOSED"; runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME"); changeSpeedMode(NORMAL); } } @@ -495,7 +496,7 @@ void EngineerManual::ctrlBPressing() void EngineerManual::ctrlBRelease() { joint2_homed_ = false; - ROS_INFO_STREAM("joint2 homed is " << joint2_homed_); + b_pressed_ = false; } void EngineerManual::ctrlCPress() From 0f16809c07b8346da23310feee4eadc6ec67ae84 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 15 May 2024 01:06:09 +0800 Subject: [PATCH 14/42] fix bugs. --- include/rm_manual/engineer_manual.h | 11 +++--- src/engineer_manual.cpp | 57 +++++++++++++++++++---------- 2 files changed, 42 insertions(+), 26 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 6e544029..fa9810a0 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -135,19 +135,21 @@ class EngineerManual : public ChassisGimbalManual void shiftVRelease(); void shiftXPress(); void shiftZPress(); + void shiftZRelease(); void mouseLeftRelease(); void mouseRightRelease(); // Servo - bool change_flag_{}, ore_lifter_on_{ false }, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, - joint2_homed_{ false }, b_pressed_{ false }; + bool change_flag_{}, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, joint2_homed_{ false }, + b_pressed_{ 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" }, last_ore_{}; - int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_height_{ 0 }, gimbal_direction_{ 0 }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_height_{ 0 }, gimbal_direction_{ 0 }, + ore_lifter_pos_{ 0 }; std::stack stone_num_{}; @@ -160,9 +162,6 @@ class EngineerManual : public ChassisGimbalManual rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; - rm_common::JointPositionBinaryCommandSender *extend_arm_a_command_sender_, *extend_arm_b_command_sender_; - rm_common::JointPointCommandSender *ore_bin_lifter_command_sender_, *ore_bin_rotate_command_sender_, - *gimbal_lifter_command_sender_; rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}, *joint2_calibration_{}; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index c50e81a1..8fb87915 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -33,8 +33,6 @@ 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); @@ -100,6 +98,7 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) 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)); @@ -289,6 +288,7 @@ 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") { @@ -303,7 +303,7 @@ void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& 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_ == "GET_UP_STONE_BIN" || prefix_ + root_ == "EXCHANGE_L" || prefix_ + root_ == "EXCHANGE_R") { enterServo(); changeSpeedMode(EXCHANGE); @@ -445,7 +445,7 @@ void EngineerManual::mouseRightRelease() //------------------------- ctrl ------------------------------ void EngineerManual::ctrlAPress() { - if (stone_num_.empty()) + if (stone_num_.size() == 0) { prefix_ = "0_TAKE_ONE_STONE_"; } @@ -684,19 +684,24 @@ void EngineerManual::rPress() void EngineerManual::vPressing() { - if (!ore_rotator_pos_) + if (!v_pressed_) { - runStepQueue("ORE_ROTATOR_EXCHANGE"); - ore_rotator_pos_ = true; - } - else - { - runStepQueue("ORE_ROTATOR_INIT"); - ore_rotator_pos_ = false; + 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() @@ -910,16 +915,28 @@ void EngineerManual::shiftXPress() void EngineerManual::shiftZPress() { - if (!ore_lifter_on_) - { - runStepQueue("ORE_LIFTER_MID"); - ore_lifter_on_ = true; - } - else + if (!shift_z_pressed_) { - runStepQueue("ORE_LIFTER_DOWN"); - ore_lifter_on_ = false; + 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 From e1cf5757824f557e6f76612c1036326b785ce5f7 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 3 Jun 2024 22:09:33 +0800 Subject: [PATCH 15/42] add scara manual for 2024 scara engineer. --- CMakeLists.txt | 4 +++- include/rm_manual/scara_manual.h | 8 ++++++++ src/manual_base.cpp | 6 ++++-- src/scara_manual.cpp | 3 +++ 4 files changed, 18 insertions(+), 3 deletions(-) create mode 100644 include/rm_manual/scara_manual.h create mode 100644 src/scara_manual.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b85422b7..6521ff0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,9 @@ include_directories( ## Declare cpp executables FILE(GLOB ALL_SOURCES "src/*.cpp") -add_executable(${PROJECT_NAME} ${ALL_SOURCES}) +add_executable(${PROJECT_NAME} ${ALL_SOURCES} + include/rm_manual/scara_manual.h + src/scara_manual.cpp) ## Add dependencies to exported targets, like ROS msgs or srvs add_dependencies(${PROJECT_NAME} diff --git a/include/rm_manual/scara_manual.h b/include/rm_manual/scara_manual.h new file mode 100644 index 00000000..bcf28cde --- /dev/null +++ b/include/rm_manual/scara_manual.h @@ -0,0 +1,8 @@ +// +// Created by cch on 24-5-31. +// + +#ifndef RM_WS_SCARA_MANUAL_H +#define RM_WS_SCARA_MANUAL_H + +#endif // RM_WS_SCARA_MANUAL_H diff --git a/src/manual_base.cpp b/src/manual_base.cpp index f3db6d50..7e614c23 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_) { diff --git a/src/scara_manual.cpp b/src/scara_manual.cpp new file mode 100644 index 00000000..56b37055 --- /dev/null +++ b/src/scara_manual.cpp @@ -0,0 +1,3 @@ +// +// Created by cch on 24-5-31. +// From 233e7dfd2b275b7434b7de2f576946e1f2315d03 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 8 Jun 2024 16:04:48 +0800 Subject: [PATCH 16/42] add basic manual codes for scara engineer. --- CMakeLists.txt | 4 +- include/rm_manual/scara_manual.h | 178 ++++++++++++- src/engineer_manual.cpp | 3 +- src/main.cpp | 3 + src/scara_manual.cpp | 411 +++++++++++++++++++++++++++++++ 5 files changed, 590 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6521ff0e..b85422b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,9 +66,7 @@ include_directories( ## Declare cpp executables FILE(GLOB ALL_SOURCES "src/*.cpp") -add_executable(${PROJECT_NAME} ${ALL_SOURCES} - include/rm_manual/scara_manual.h - src/scara_manual.cpp) +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) ## Add dependencies to exported targets, like ROS msgs or srvs add_dependencies(${PROJECT_NAME} diff --git a/include/rm_manual/scara_manual.h b/include/rm_manual/scara_manual.h index bcf28cde..948861a6 100644 --- a/include/rm_manual/scara_manual.h +++ b/include/rm_manual/scara_manual.h @@ -1,8 +1,178 @@ // -// Created by cch on 24-5-31. +// Created by qiayuan on 5/23/21. // -#ifndef RM_WS_SCARA_MANUAL_H -#define RM_WS_SCARA_MANUAL_H +#pragma once -#endif // RM_WS_SCARA_MANUAL_H +#include "chassis_gimbal_manual.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rm_manual +{ +class ScaraManual : public ChassisGimbalManual +{ +public: + enum ControlMode + { + MANUAL, + MIDDLEWARE + }; + + enum JointMode + { + SERVO, + JOINT + }; + + enum GimbalMode + { + RATE, + DIRECT + }; + + enum SpeedMode + { + LOW, + NORMAL, + FAST, + EXCHANGE + }; + + ScaraManual(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 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 change_flag_{}, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, joint2_homed_{ false }, + b_pressed_{ 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" }, last_ore_{}; + int operating_mode_{}, servo_mode_{}, 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_, old_ui_; + + rm_common::Vel3DCommandSender* servo_command_sender_; + rm_common::ServiceCallerBase* servo_reset_caller_; + rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}, + *joint2_calibration_{}; + + actionlib::SimpleActionClient action_client_; + + InputEvent left_switch_up_event_, left_switch_down_event_, ctrl_a_event_, ctrl_b_event_, ctrl_c_event_, ctrl_d_event_, + ctrl_e_event_, ctrl_f_event_, ctrl_g_event_, ctrl_q_event_, ctrl_r_event_, ctrl_s_event_, ctrl_v_event_, + ctrl_w_event_, ctrl_x_event_, ctrl_z_event_, b_event_, c_event_, e_event_, f_event_, g_event_, q_event_, r_event_, + v_event_, x_event_, z_event_, shift_event_, shift_b_event_, shift_c_event_, shift_e_event_, shift_f_event_, + shift_g_event_, shift_v_event_, shift_q_event_, shift_r_event_, shift_x_event_, shift_z_event_, mouse_left_event_, + mouse_right_event_; +}; + +} // namespace rm_manual diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 8fb87915..1bd8d48b 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -434,13 +434,12 @@ 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() diff --git a/src/main.cpp b/src/main.cpp index b515ec08..958479fe 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/scara_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::ScaraManual(nh, nh_referee); else if (robot == "dart") manual_control = new rm_manual::DartManual(nh, nh_referee); else if (robot == "balance") diff --git a/src/scara_manual.cpp b/src/scara_manual.cpp index 56b37055..c4d71205 100644 --- a/src/scara_manual.cpp +++ b/src/scara_manual.cpp @@ -1,3 +1,414 @@ // // Created by cch on 24-5-31. // +#include "rm_manual/scara_manual.h" + +namespace rm_manual +{ +ScaraManual::ScaraManual(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, &ScaraManual::stoneNumCallback, this); + gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, + &ScaraManual::gpioStateCallback, this); + + // Servo + ros::NodeHandle nh_servo(nh, "servo"); + servo_command_sender_ = new rm_common::Vel3DCommandSender(nh_servo); + servo_reset_caller_ = new rm_common::ServiceCallerBase(nh_servo, "/servo_server/reset_servo_status"); + // Vel + ros::NodeHandle chassis_nh(nh, "chassis"); + chassis_nh.param("fast_speed_scale", fast_speed_scale_, 1.0); + chassis_nh.param("normal_speed_scale", normal_speed_scale_, 0.5); + chassis_nh.param("low_speed_scale", low_speed_scale_, 0.3); + chassis_nh.param("exchange_speed_scale", exchange_speed_scale_, 0.2); + chassis_nh.param("fast_gyro_scale", fast_gyro_scale_, 0.5); + chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); + chassis_nh.param("low_gyro_scale", low_gyro_scale_, 0.05); + chassis_nh.param("exchange_gyro_scale", exchange_gyro_scale_, 0.12); + // 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(&ScaraManual::leftSwitchUpFall, this)); + left_switch_up_event_.setRising(boost::bind(&ScaraManual::leftSwitchUpRise, this)); + left_switch_down_event_.setFalling(boost::bind(&ScaraManual::leftSwitchDownFall, this)); + left_switch_down_event_.setRising(boost::bind(&ScaraManual::leftSwitchDownRise, this)); + ctrl_a_event_.setRising(boost::bind(&ScaraManual::ctrlAPress, this)); + ctrl_b_event_.setRising(boost::bind(&ScaraManual::ctrlBPress, this)); + ctrl_c_event_.setRising(boost::bind(&ScaraManual::ctrlCPress, this)); + ctrl_b_event_.setActiveHigh(boost::bind(&ScaraManual::ctrlBPressing, this)); + ctrl_b_event_.setFalling(boost::bind(&ScaraManual::ctrlBRelease, this)); + ctrl_d_event_.setRising(boost::bind(&ScaraManual::ctrlDPress, this)); + ctrl_e_event_.setRising(boost::bind(&ScaraManual::ctrlEPress, this)); + ctrl_f_event_.setRising(boost::bind(&ScaraManual::ctrlFPress, this)); + ctrl_g_event_.setRising(boost::bind(&ScaraManual::ctrlGPress, this)); + ctrl_q_event_.setRising(boost::bind(&ScaraManual::ctrlQPress, this)); + ctrl_r_event_.setRising(boost::bind(&ScaraManual::ctrlRPress, this)); + ctrl_s_event_.setRising(boost::bind(&ScaraManual::ctrlSPress, this)); + ctrl_v_event_.setRising(boost::bind(&ScaraManual::ctrlVPress, this)); + ctrl_v_event_.setFalling(boost::bind(&ScaraManual::ctrlVRelease, this)); + ctrl_w_event_.setRising(boost::bind(&ScaraManual::ctrlWPress, this)); + ctrl_x_event_.setRising(boost::bind(&ScaraManual::ctrlXPress, this)); + ctrl_z_event_.setRising(boost::bind(&ScaraManual::ctrlZPress, this)); + b_event_.setActiveHigh(boost::bind(&ScaraManual::bPressing, this)); + b_event_.setFalling(boost::bind(&ScaraManual::bRelease, this)); + c_event_.setActiveHigh(boost::bind(&ScaraManual::cPressing, this)); + c_event_.setFalling(boost::bind(&ScaraManual::cRelease, this)); + e_event_.setActiveHigh(boost::bind(&ScaraManual::ePressing, this)); + e_event_.setFalling(boost::bind(&ScaraManual::eRelease, this)); + f_event_.setRising(boost::bind(&ScaraManual::fPress, this)); + f_event_.setFalling(boost::bind(&ScaraManual::fRelease, this)); + g_event_.setRising(boost::bind(&ScaraManual::gPress, this)); + g_event_.setFalling(boost::bind(&ScaraManual::gRelease, this)); + q_event_.setActiveHigh(boost::bind(&ScaraManual::qPressing, this)); + q_event_.setFalling(boost::bind(&ScaraManual::qRelease, this)); + r_event_.setRising(boost::bind(&ScaraManual::rPress, this)); + v_event_.setActiveHigh(boost::bind(&ScaraManual::vPressing, this)); + v_event_.setFalling(boost::bind(&ScaraManual::vRelease, this)); + x_event_.setRising(boost::bind(&ScaraManual::xPress, this)); + z_event_.setActiveHigh(boost::bind(&ScaraManual::zPressing, this)); + z_event_.setFalling(boost::bind(&ScaraManual::zRelease, this)); + shift_event_.setActiveHigh(boost::bind(&ScaraManual::shiftPressing, this)); + shift_event_.setFalling(boost::bind(&ScaraManual::shiftRelease, this)); + shift_b_event_.setRising(boost::bind(&ScaraManual::shiftBPress, this)); + shift_b_event_.setFalling(boost::bind(&ScaraManual::shiftBRelease, this)); + shift_c_event_.setRising(boost::bind(&ScaraManual::shiftCPress, this)); + shift_e_event_.setRising(boost::bind(&ScaraManual::shiftEPress, this)); + shift_f_event_.setRising(boost::bind(&ScaraManual::shiftFPress, this)); + shift_g_event_.setRising(boost::bind(&ScaraManual::shiftGPress, this)); + shift_q_event_.setRising(boost::bind(&ScaraManual::shiftQPress, this)); + shift_r_event_.setRising(boost::bind(&ScaraManual::shiftRPress, this)); + shift_r_event_.setFalling(boost::bind(&ScaraManual::shiftRRelease, this)); + shift_v_event_.setRising(boost::bind(&ScaraManual::shiftVPress, this)); + shift_v_event_.setFalling(boost::bind(&ScaraManual::shiftVRelease, this)); + shift_x_event_.setRising(boost::bind(&ScaraManual::shiftXPress, this)); + shift_z_event_.setRising(boost::bind(&ScaraManual::shiftZPress, this)); + shift_z_event_.setFalling(boost::bind(&ScaraManual::shiftZRelease, this)); + + mouse_left_event_.setFalling(boost::bind(&ScaraManual::mouseLeftRelease, this)); + mouse_right_event_.setFalling(boost::bind(&ScaraManual::mouseRightRelease, this)); +} + +void ScaraManual::run() +{ + ChassisGimbalManual::run(); + calibration_gather_->update(ros::Time::now()); + if (engineer_ui_ != old_ui_) + { + engineer_ui_pub_.publish(engineer_ui_); + old_ui_ = engineer_ui_; + } +} + +void ScaraManual::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; + default: + speed_change_scale_ = normal_speed_scale_; + gyro_scale_ = normal_gyro_scale_; + break; + } +} + +void ScaraManual::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_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 ScaraManual::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 ScaraManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + checkKeyboard(dbus_data); + left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + if (servo_mode_ == JOINT) + vel_cmd_sender_->setAngularZVel(-dbus_data->m_x * gimbal_scale_); +} + +void ScaraManual::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 ScaraManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) +{ + ManualBase::dbusDataCallback(data); + chassis_cmd_sender_->updateRefereeStatus(referee_is_online_); + if (servo_mode_ == SERVO) + updateServo(data); +} + +void ScaraManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) +{ +} + +void ScaraManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) +{ + gpio_state_.gpio_state = data->gpio_state; + if (gpio_state_.gpio_state[0]) + { + gripper_state_ = "open"; + engineer_ui_.gripper_state = "OPEN"; + } + else + { + gripper_state_ = "close"; + engineer_ui_.gripper_state = "CLOSED"; + } +} + +void ScaraManual::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 ScaraManual::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(&ScaraManual::actionDoneCallback, this, _1, _2), + boost::bind(&ScaraManual::actionActiveCallback, this), + boost::bind(&ScaraManual::actionFeedbackCallback, this, _1)); + operating_mode_ = MIDDLEWARE; + } + else + ROS_ERROR("Can not connect to middleware"); +} + +void ScaraManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) +{ +} + +void ScaraManual::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()); + change_flag_ = true; + ROS_INFO("%i", result->finish); + operating_mode_ = MANUAL; + if (root_ == "HOME") + { + initMode(); + changeSpeedMode(NORMAL); + } +} + +void ScaraManual::enterServo() +{ + servo_mode_ = SERVO; + gimbal_mode_ = DIRECT; + changeSpeedMode(EXCHANGE); + servo_reset_caller_->callService(); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + action_client_.cancelAllGoals(); + chassis_cmd_sender_->getMsg()->command_source_frame = "link4"; + engineer_ui_.control_mode = "SERVO"; +} + +void ScaraManual::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 ScaraManual::remoteControlTurnOff() +{ + ManualBase::remoteControlTurnOff(); + action_client_.cancelAllGoals(); +} + +void ScaraManual::gimbalOutputOn() +{ + ChassisGimbalManual::gimbalOutputOn(); +} + +void ScaraManual::chassisOutputOn() +{ + if (operating_mode_ == MIDDLEWARE) + action_client_.cancelAllGoals(); +} + +//-------------------controller input------------------- +void ScaraManual::rightSwitchUpRise() +{ + ChassisGimbalManual::rightSwitchUpRise(); + gimbal_mode_ = DIRECT; + servo_mode_ = JOINT; + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); +} +void ScaraManual::rightSwitchMidRise() +{ + ChassisGimbalManual::rightSwitchMidRise(); + servo_mode_ = JOINT; + gimbal_mode_ = RATE; + gimbal_cmd_sender_->setZero(); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); +} +void ScaraManual::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 ScaraManual::leftSwitchUpRise() +{ + prefix_ = ""; + root_ = "CALIBRATION"; + calibration_gather_->reset(); + engineer_ui_.stone_num = 0; + engineer_ui_.gripper_state = "CLOSED"; + engineer_ui_.control_mode = "NORMAL"; + ROS_INFO_STREAM("START CALIBRATE"); +} +void ScaraManual::leftSwitchUpFall() +{ + runStepQueue("HOME"); + runStepQueue("CLOSE_GRIPPER"); +} + +void ScaraManual::leftSwitchDownRise() +{ + if (gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + engineer_ui_.gripper_state = "OPEN"; + } + else + { + runStepQueue("CLOSE_GRIPPER"); + engineer_ui_.gripper_state = "CLOSED"; + } +} +void ScaraManual::leftSwitchDownFall() +{ +} + +//--------------------- keyboard input ------------------------ +// mouse input +void ScaraManual::mouseLeftRelease() +{ + if (change_flag_) + { + root_ += "0"; + change_flag_ = false; + runStepQueue(prefix_ + root_); + ROS_INFO("Finished %s", (prefix_ + root_).c_str()); + } +} + +void ScaraManual::mouseRightRelease() +{ + runStepQueue(prefix_ + root_); + ROS_INFO("Finished %s", (prefix_ + root_).c_str()); +} + +void ScaraManual::bPressing() +{ +} + +void ScaraManual::bRelease() +{ +} +} // namespace rm_manual From caea82e163cc32d6f9ea13c420891cf4fa39c055 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 9 Jul 2024 21:49:47 +0800 Subject: [PATCH 17/42] record. --- src/scara_manual.cpp | 153 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 153 insertions(+) diff --git a/src/scara_manual.cpp b/src/scara_manual.cpp index c4d71205..94684cf6 100644 --- a/src/scara_manual.cpp +++ b/src/scara_manual.cpp @@ -411,4 +411,157 @@ void ScaraManual::bPressing() void ScaraManual::bRelease() { } +void ScaraManual::cPressing() +{ +} +void ScaraManual::cRelease() +{ +} +void ScaraManual::ePressing() +{ +} +void ScaraManual::eRelease() +{ +} +void ScaraManual::fPress() +{ +} +void ScaraManual::fRelease() +{ +} +void ScaraManual::gPress() +{ +} +void ScaraManual::gRelease() +{ +} +void ScaraManual::qPressing() +{ +} +void ScaraManual::qRelease() +{ +} +void ScaraManual::rPress() +{ +} +void ScaraManual::vPressing() +{ +} +void ScaraManual::vRelease() +{ +} +void ScaraManual::xPress() +{ +} +void ScaraManual::zPressing() +{ +} +void ScaraManual::zRelease() +{ +} + +//--------------------- CTRL --------------------- +void ScaraManual::ctrlAPress() +{ +} +void ScaraManual::ctrlBPress() +{ +} +void ScaraManual::ctrlBPressing() +{ +} +void ScaraManual::ctrlBRelease() +{ +} +void ScaraManual::ctrlCPress() +{ +} +void ScaraManual::ctrlDPress() +{ +} +void ScaraManual::ctrlEPress() +{ +} +void ScaraManual::ctrlFPress() +{ +} +void ScaraManual::ctrlGPress() +{ +} +void ScaraManual::ctrlQPress() +{ +} +void ScaraManual::ctrlRPress() +{ +} +void ScaraManual::ctrlSPress() +{ +} +void ScaraManual::ctrlVPress() +{ +} +void ScaraManual::ctrlVRelease() +{ +} +void ScaraManual::ctrlWPress() +{ +} +void ScaraManual::ctrlXPress() +{ +} +void ScaraManual::ctrlZPress() +{ +} + +//--------------- SHIFT -------------------------- + +void ScaraManual::shiftPressing() +{ +} +void ScaraManual::shiftRelease() +{ +} +void ScaraManual::shiftBPress() +{ +} +void ScaraManual::shiftBRelease() +{ +} +void ScaraManual::shiftCPress() +{ +} +void ScaraManual::shiftEPress() +{ +} +void ScaraManual::shiftFPress() +{ +} +void ScaraManual::shiftGPress() +{ +} +void ScaraManual::shiftQPress() +{ +} +void ScaraManual::shiftRPress() +{ +} +void ScaraManual::shiftRRelease() +{ +} +void ScaraManual::shiftVPress() +{ +} +void ScaraManual::shiftVRelease() +{ +} +void ScaraManual::shiftXPress() +{ +} +void ScaraManual::shiftZPress() +{ +} +void ScaraManual::shiftZRelease() +{ +} + } // namespace rm_manual From 612bdf72d977dbe20b446555848c337d26340a40 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 13 Jul 2024 03:45:54 +0800 Subject: [PATCH 18/42] Add engineer2 in robot type. --- launch/load.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 @@ - + From ab9189830bfc648243e91c0ba91a25376d72fc19 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 15 Jul 2024 12:05:06 +0800 Subject: [PATCH 19/42] Rename engineer2 manual from scara manual. --- .../{scara_manual.h => engineer2_manual.h} | 4 +- ...{scara_manual.cpp => engineer2_manual.cpp} | 292 +++++++++--------- src/main.cpp | 4 +- 3 files changed, 150 insertions(+), 150 deletions(-) rename include/rm_manual/{scara_manual.h => engineer2_manual.h} (97%) rename src/{scara_manual.cpp => engineer2_manual.cpp} (55%) diff --git a/include/rm_manual/scara_manual.h b/include/rm_manual/engineer2_manual.h similarity index 97% rename from include/rm_manual/scara_manual.h rename to include/rm_manual/engineer2_manual.h index 948861a6..6b08c58e 100644 --- a/include/rm_manual/scara_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -20,7 +20,7 @@ namespace rm_manual { -class ScaraManual : public ChassisGimbalManual +class Engineer2Manual : public ChassisGimbalManual { public: enum ControlMode @@ -49,7 +49,7 @@ class ScaraManual : public ChassisGimbalManual EXCHANGE }; - ScaraManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); + Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); void run() override; private: diff --git a/src/scara_manual.cpp b/src/engineer2_manual.cpp similarity index 55% rename from src/scara_manual.cpp rename to src/engineer2_manual.cpp index 94684cf6..9799883c 100644 --- a/src/scara_manual.cpp +++ b/src/engineer2_manual.cpp @@ -1,11 +1,11 @@ // // Created by cch on 24-5-31. // -#include "rm_manual/scara_manual.h" +#include "rm_manual/engineer2_manual.h" namespace rm_manual { -ScaraManual::ScaraManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) +Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalManual(nh, nh_referee) , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) @@ -14,9 +14,9 @@ ScaraManual::ScaraManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); - stone_num_sub_ = nh.subscribe("/stone_num", 10, &ScaraManual::stoneNumCallback, this); + stone_num_sub_ = nh.subscribe("/stone_num", 10, &Engineer2Manual::stoneNumCallback, this); gripper_state_sub_ = nh.subscribe("/controllers/gpio_controller/gpio_states", 10, - &ScaraManual::gpioStateCallback, this); + &Engineer2Manual::gpioStateCallback, this); // Servo ros::NodeHandle nh_servo(nh, "servo"); @@ -36,67 +36,67 @@ ScaraManual::ScaraManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) 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(&ScaraManual::leftSwitchUpFall, this)); - left_switch_up_event_.setRising(boost::bind(&ScaraManual::leftSwitchUpRise, this)); - left_switch_down_event_.setFalling(boost::bind(&ScaraManual::leftSwitchDownFall, this)); - left_switch_down_event_.setRising(boost::bind(&ScaraManual::leftSwitchDownRise, this)); - ctrl_a_event_.setRising(boost::bind(&ScaraManual::ctrlAPress, this)); - ctrl_b_event_.setRising(boost::bind(&ScaraManual::ctrlBPress, this)); - ctrl_c_event_.setRising(boost::bind(&ScaraManual::ctrlCPress, this)); - ctrl_b_event_.setActiveHigh(boost::bind(&ScaraManual::ctrlBPressing, this)); - ctrl_b_event_.setFalling(boost::bind(&ScaraManual::ctrlBRelease, this)); - ctrl_d_event_.setRising(boost::bind(&ScaraManual::ctrlDPress, this)); - ctrl_e_event_.setRising(boost::bind(&ScaraManual::ctrlEPress, this)); - ctrl_f_event_.setRising(boost::bind(&ScaraManual::ctrlFPress, this)); - ctrl_g_event_.setRising(boost::bind(&ScaraManual::ctrlGPress, this)); - ctrl_q_event_.setRising(boost::bind(&ScaraManual::ctrlQPress, this)); - ctrl_r_event_.setRising(boost::bind(&ScaraManual::ctrlRPress, this)); - ctrl_s_event_.setRising(boost::bind(&ScaraManual::ctrlSPress, this)); - ctrl_v_event_.setRising(boost::bind(&ScaraManual::ctrlVPress, this)); - ctrl_v_event_.setFalling(boost::bind(&ScaraManual::ctrlVRelease, this)); - ctrl_w_event_.setRising(boost::bind(&ScaraManual::ctrlWPress, this)); - ctrl_x_event_.setRising(boost::bind(&ScaraManual::ctrlXPress, this)); - ctrl_z_event_.setRising(boost::bind(&ScaraManual::ctrlZPress, this)); - b_event_.setActiveHigh(boost::bind(&ScaraManual::bPressing, this)); - b_event_.setFalling(boost::bind(&ScaraManual::bRelease, this)); - c_event_.setActiveHigh(boost::bind(&ScaraManual::cPressing, this)); - c_event_.setFalling(boost::bind(&ScaraManual::cRelease, this)); - e_event_.setActiveHigh(boost::bind(&ScaraManual::ePressing, this)); - e_event_.setFalling(boost::bind(&ScaraManual::eRelease, this)); - f_event_.setRising(boost::bind(&ScaraManual::fPress, this)); - f_event_.setFalling(boost::bind(&ScaraManual::fRelease, this)); - g_event_.setRising(boost::bind(&ScaraManual::gPress, this)); - g_event_.setFalling(boost::bind(&ScaraManual::gRelease, this)); - q_event_.setActiveHigh(boost::bind(&ScaraManual::qPressing, this)); - q_event_.setFalling(boost::bind(&ScaraManual::qRelease, this)); - r_event_.setRising(boost::bind(&ScaraManual::rPress, this)); - v_event_.setActiveHigh(boost::bind(&ScaraManual::vPressing, this)); - v_event_.setFalling(boost::bind(&ScaraManual::vRelease, this)); - x_event_.setRising(boost::bind(&ScaraManual::xPress, this)); - z_event_.setActiveHigh(boost::bind(&ScaraManual::zPressing, this)); - z_event_.setFalling(boost::bind(&ScaraManual::zRelease, this)); - shift_event_.setActiveHigh(boost::bind(&ScaraManual::shiftPressing, this)); - shift_event_.setFalling(boost::bind(&ScaraManual::shiftRelease, this)); - shift_b_event_.setRising(boost::bind(&ScaraManual::shiftBPress, this)); - shift_b_event_.setFalling(boost::bind(&ScaraManual::shiftBRelease, this)); - shift_c_event_.setRising(boost::bind(&ScaraManual::shiftCPress, this)); - shift_e_event_.setRising(boost::bind(&ScaraManual::shiftEPress, this)); - shift_f_event_.setRising(boost::bind(&ScaraManual::shiftFPress, this)); - shift_g_event_.setRising(boost::bind(&ScaraManual::shiftGPress, this)); - shift_q_event_.setRising(boost::bind(&ScaraManual::shiftQPress, this)); - shift_r_event_.setRising(boost::bind(&ScaraManual::shiftRPress, this)); - shift_r_event_.setFalling(boost::bind(&ScaraManual::shiftRRelease, this)); - shift_v_event_.setRising(boost::bind(&ScaraManual::shiftVPress, this)); - shift_v_event_.setFalling(boost::bind(&ScaraManual::shiftVRelease, this)); - shift_x_event_.setRising(boost::bind(&ScaraManual::shiftXPress, this)); - shift_z_event_.setRising(boost::bind(&ScaraManual::shiftZPress, this)); - shift_z_event_.setFalling(boost::bind(&ScaraManual::shiftZRelease, this)); - - mouse_left_event_.setFalling(boost::bind(&ScaraManual::mouseLeftRelease, this)); - mouse_right_event_.setFalling(boost::bind(&ScaraManual::mouseRightRelease, this)); -} - -void ScaraManual::run() + 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)); + 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()); @@ -107,7 +107,7 @@ void ScaraManual::run() } } -void ScaraManual::changeSpeedMode(SpeedMode speed_mode) +void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) { switch (speed_mode) { @@ -134,7 +134,7 @@ void ScaraManual::changeSpeedMode(SpeedMode speed_mode) } } -void ScaraManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) +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); @@ -180,7 +180,7 @@ void ScaraManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); } -void ScaraManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) +void Engineer2Manual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::updateRc(dbus_data); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); @@ -192,7 +192,7 @@ void ScaraManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) left_switch_down_event_.update(dbus_data->s_l == rm_msgs::DbusData::DOWN); } -void ScaraManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) +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); @@ -201,13 +201,13 @@ void ScaraManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) vel_cmd_sender_->setAngularZVel(-dbus_data->m_x * gimbal_scale_); } -void ScaraManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) +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 ScaraManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) +void Engineer2Manual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { ManualBase::dbusDataCallback(data); chassis_cmd_sender_->updateRefereeStatus(referee_is_online_); @@ -215,11 +215,11 @@ void ScaraManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) updateServo(data); } -void ScaraManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) +void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { } -void ScaraManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) +void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { gpio_state_.gpio_state = data->gpio_state; if (gpio_state_.gpio_state[0]) @@ -234,7 +234,7 @@ void ScaraManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) } } -void ScaraManual::sendCommand(const ros::Time& time) +void Engineer2Manual::sendCommand(const ros::Time& time) { if (operating_mode_ == MANUAL) { @@ -250,28 +250,28 @@ void ScaraManual::sendCommand(const ros::Time& time) } } -void ScaraManual::runStepQueue(const std::string& step_queue_name) +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(&ScaraManual::actionDoneCallback, this, _1, _2), - boost::bind(&ScaraManual::actionActiveCallback, this), - boost::bind(&ScaraManual::actionFeedbackCallback, this, _1)); + 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 ScaraManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) +void Engineer2Manual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) { } -void ScaraManual::actionDoneCallback(const actionlib::SimpleClientGoalState& state, - const rm_msgs::EngineerResultConstPtr& result) +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); @@ -286,7 +286,7 @@ void ScaraManual::actionDoneCallback(const actionlib::SimpleClientGoalState& sta } } -void ScaraManual::enterServo() +void Engineer2Manual::enterServo() { servo_mode_ = SERVO; gimbal_mode_ = DIRECT; @@ -298,7 +298,7 @@ void ScaraManual::enterServo() engineer_ui_.control_mode = "SERVO"; } -void ScaraManual::initMode() +void Engineer2Manual::initMode() { servo_mode_ = JOINT; gimbal_mode_ = DIRECT; @@ -308,32 +308,32 @@ void ScaraManual::initMode() engineer_ui_.control_mode = "NORMAL"; } -void ScaraManual::remoteControlTurnOff() +void Engineer2Manual::remoteControlTurnOff() { ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } -void ScaraManual::gimbalOutputOn() +void Engineer2Manual::gimbalOutputOn() { ChassisGimbalManual::gimbalOutputOn(); } -void ScaraManual::chassisOutputOn() +void Engineer2Manual::chassisOutputOn() { if (operating_mode_ == MIDDLEWARE) action_client_.cancelAllGoals(); } //-------------------controller input------------------- -void ScaraManual::rightSwitchUpRise() +void Engineer2Manual::rightSwitchUpRise() { ChassisGimbalManual::rightSwitchUpRise(); gimbal_mode_ = DIRECT; servo_mode_ = JOINT; chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } -void ScaraManual::rightSwitchMidRise() +void Engineer2Manual::rightSwitchMidRise() { ChassisGimbalManual::rightSwitchMidRise(); servo_mode_ = JOINT; @@ -341,7 +341,7 @@ void ScaraManual::rightSwitchMidRise() gimbal_cmd_sender_->setZero(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } -void ScaraManual::rightSwitchDownRise() +void Engineer2Manual::rightSwitchDownRise() { ChassisGimbalManual::rightSwitchDownRise(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); @@ -352,7 +352,7 @@ void ScaraManual::rightSwitchDownRise() ROS_INFO_STREAM("servo_mode"); } -void ScaraManual::leftSwitchUpRise() +void Engineer2Manual::leftSwitchUpRise() { prefix_ = ""; root_ = "CALIBRATION"; @@ -362,13 +362,13 @@ void ScaraManual::leftSwitchUpRise() engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } -void ScaraManual::leftSwitchUpFall() +void Engineer2Manual::leftSwitchUpFall() { runStepQueue("HOME"); runStepQueue("CLOSE_GRIPPER"); } -void ScaraManual::leftSwitchDownRise() +void Engineer2Manual::leftSwitchDownRise() { if (gripper_state_ == "close") { @@ -381,13 +381,13 @@ void ScaraManual::leftSwitchDownRise() engineer_ui_.gripper_state = "CLOSED"; } } -void ScaraManual::leftSwitchDownFall() +void Engineer2Manual::leftSwitchDownFall() { } //--------------------- keyboard input ------------------------ // mouse input -void ScaraManual::mouseLeftRelease() +void Engineer2Manual::mouseLeftRelease() { if (change_flag_) { @@ -398,169 +398,169 @@ void ScaraManual::mouseLeftRelease() } } -void ScaraManual::mouseRightRelease() +void Engineer2Manual::mouseRightRelease() { runStepQueue(prefix_ + root_); ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } -void ScaraManual::bPressing() +void Engineer2Manual::bPressing() { } -void ScaraManual::bRelease() +void Engineer2Manual::bRelease() { } -void ScaraManual::cPressing() +void Engineer2Manual::cPressing() { } -void ScaraManual::cRelease() +void Engineer2Manual::cRelease() { } -void ScaraManual::ePressing() +void Engineer2Manual::ePressing() { } -void ScaraManual::eRelease() +void Engineer2Manual::eRelease() { } -void ScaraManual::fPress() +void Engineer2Manual::fPress() { } -void ScaraManual::fRelease() +void Engineer2Manual::fRelease() { } -void ScaraManual::gPress() +void Engineer2Manual::gPress() { } -void ScaraManual::gRelease() +void Engineer2Manual::gRelease() { } -void ScaraManual::qPressing() +void Engineer2Manual::qPressing() { } -void ScaraManual::qRelease() +void Engineer2Manual::qRelease() { } -void ScaraManual::rPress() +void Engineer2Manual::rPress() { } -void ScaraManual::vPressing() +void Engineer2Manual::vPressing() { } -void ScaraManual::vRelease() +void Engineer2Manual::vRelease() { } -void ScaraManual::xPress() +void Engineer2Manual::xPress() { } -void ScaraManual::zPressing() +void Engineer2Manual::zPressing() { } -void ScaraManual::zRelease() +void Engineer2Manual::zRelease() { } //--------------------- CTRL --------------------- -void ScaraManual::ctrlAPress() +void Engineer2Manual::ctrlAPress() { } -void ScaraManual::ctrlBPress() +void Engineer2Manual::ctrlBPress() { } -void ScaraManual::ctrlBPressing() +void Engineer2Manual::ctrlBPressing() { } -void ScaraManual::ctrlBRelease() +void Engineer2Manual::ctrlBRelease() { } -void ScaraManual::ctrlCPress() +void Engineer2Manual::ctrlCPress() { } -void ScaraManual::ctrlDPress() +void Engineer2Manual::ctrlDPress() { } -void ScaraManual::ctrlEPress() +void Engineer2Manual::ctrlEPress() { } -void ScaraManual::ctrlFPress() +void Engineer2Manual::ctrlFPress() { } -void ScaraManual::ctrlGPress() +void Engineer2Manual::ctrlGPress() { } -void ScaraManual::ctrlQPress() +void Engineer2Manual::ctrlQPress() { } -void ScaraManual::ctrlRPress() +void Engineer2Manual::ctrlRPress() { } -void ScaraManual::ctrlSPress() +void Engineer2Manual::ctrlSPress() { } -void ScaraManual::ctrlVPress() +void Engineer2Manual::ctrlVPress() { } -void ScaraManual::ctrlVRelease() +void Engineer2Manual::ctrlVRelease() { } -void ScaraManual::ctrlWPress() +void Engineer2Manual::ctrlWPress() { } -void ScaraManual::ctrlXPress() +void Engineer2Manual::ctrlXPress() { } -void ScaraManual::ctrlZPress() +void Engineer2Manual::ctrlZPress() { } //--------------- SHIFT -------------------------- -void ScaraManual::shiftPressing() +void Engineer2Manual::shiftPressing() { } -void ScaraManual::shiftRelease() +void Engineer2Manual::shiftRelease() { } -void ScaraManual::shiftBPress() +void Engineer2Manual::shiftBPress() { } -void ScaraManual::shiftBRelease() +void Engineer2Manual::shiftBRelease() { } -void ScaraManual::shiftCPress() +void Engineer2Manual::shiftCPress() { } -void ScaraManual::shiftEPress() +void Engineer2Manual::shiftEPress() { } -void ScaraManual::shiftFPress() +void Engineer2Manual::shiftFPress() { } -void ScaraManual::shiftGPress() +void Engineer2Manual::shiftGPress() { } -void ScaraManual::shiftQPress() +void Engineer2Manual::shiftQPress() { } -void ScaraManual::shiftRPress() +void Engineer2Manual::shiftRPress() { } -void ScaraManual::shiftRRelease() +void Engineer2Manual::shiftRRelease() { } -void ScaraManual::shiftVPress() +void Engineer2Manual::shiftVPress() { } -void ScaraManual::shiftVRelease() +void Engineer2Manual::shiftVRelease() { } -void ScaraManual::shiftXPress() +void Engineer2Manual::shiftXPress() { } -void ScaraManual::shiftZPress() +void Engineer2Manual::shiftZPress() { } -void ScaraManual::shiftZRelease() +void Engineer2Manual::shiftZRelease() { } diff --git a/src/main.cpp b/src/main.cpp index 958479fe..8a10626e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +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/scara_manual.h" +#include "rm_manual/engineer2_manual.h" #include "rm_manual/dart_manual.h" #include "rm_manual/balance_manual.h" @@ -24,7 +24,7 @@ int main(int argc, char** argv) else if (robot == "engineer") manual_control = new rm_manual::EngineerManual(nh, nh_referee); else if (robot == "engineer2") - manual_control = new rm_manual::ScaraManual(nh, nh_referee); + 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") From 8257c9b5fdc886222738f677e92ea10feb744c11 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 16 Jul 2024 10:54:41 +0800 Subject: [PATCH 20/42] Add basic functions in engineer2 manual. --- include/rm_manual/engineer2_manual.h | 15 ++- src/engineer2_manual.cpp | 132 +++++++++++++++++++++++---- 2 files changed, 121 insertions(+), 26 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 6b08c58e..7b5f8750 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include namespace rm_manual @@ -142,14 +142,12 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_rotator_pos_{ false }, joint2_calibrated_{ false }, joint2_homed_{ false }, - b_pressed_{ 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_{}, gimbal_mode_{}, gimbal_height_{ 0 }, gimbal_direction_{ 0 }, - ore_lifter_pos_{ 0 }; + std::string prefix_{}, root_{}, main_gripper_state_{ "off" }, exchange_direction_{ "left" }; + int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; @@ -158,12 +156,11 @@ class Engineer2Manual : public ChassisGimbalManual ros::Publisher engineer_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::EngineerUi engineer_ui_, old_ui_; + rm_msgs::Engineer2Ui engineer_ui_, old_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; - rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}, - *joint2_calibration_{}; + rm_common::CalibrationQueue* calibration_gather_{}; actionlib::SimpleActionClient action_client_; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 9799883c..e5b78f62 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -10,7 +10,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) { - engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); + engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); @@ -222,15 +222,20 @@ void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { gpio_state_.gpio_state = data->gpio_state; - if (gpio_state_.gpio_state[0]) + for (int i = 0; i < gpio_state_.gpio_state.size(); i++) { - gripper_state_ = "open"; - engineer_ui_.gripper_state = "OPEN"; - } - else - { - gripper_state_ = "close"; - engineer_ui_.gripper_state = "CLOSED"; + if (gpio_state_.gpio_state[i] == true) + { + if (i == 0) + main_gripper_state_ = "open"; + engineer_ui_.gripper_state[i] = true; + } + else + { + if (i == 0) + main_gripper_state_ = "close"; + engineer_ui_.gripper_state[i] = false; + } } } @@ -294,7 +299,7 @@ void Engineer2Manual::enterServo() servo_reset_caller_->callService(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); action_client_.cancelAllGoals(); - chassis_cmd_sender_->getMsg()->command_source_frame = "link4"; + chassis_cmd_sender_->getMsg()->command_source_frame = "link3"; engineer_ui_.control_mode = "SERVO"; } @@ -357,8 +362,14 @@ void Engineer2Manual::leftSwitchUpRise() prefix_ = ""; root_ = "CALIBRATION"; calibration_gather_->reset(); - engineer_ui_.stone_num = 0; - engineer_ui_.gripper_state = "CLOSED"; + for (auto& stone : engineer_ui_.stone_num) + { + stone = false; + } + for (auto& gripper_state : engineer_ui_.gripper_state) + { + gripper_state = false; + } engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } @@ -370,15 +381,15 @@ void Engineer2Manual::leftSwitchUpFall() void Engineer2Manual::leftSwitchDownRise() { - if (gripper_state_ == "close") + if (main_gripper_state_ == "close") { - runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state = "OPEN"; + runStepQueue("OPEN_MAIN_GRIPPER"); + engineer_ui_.gripper_state[0] = true; } else { - runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state = "CLOSED"; + runStepQueue("CLOSE_MAIN_GRIPPER"); + engineer_ui_.gripper_state[0] = false; } } void Engineer2Manual::leftSwitchDownFall() @@ -413,15 +424,21 @@ void Engineer2Manual::bRelease() } void Engineer2Manual::cPressing() { + angular_z_scale_ = 0.6; } 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() { @@ -437,9 +454,13 @@ 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() { @@ -452,20 +473,45 @@ void Engineer2Manual::vRelease() } void Engineer2Manual::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 Engineer2Manual::zPressing() { + angular_z_scale_ = -0.6; } 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("HOME"); + changeSpeedMode(NORMAL); } void Engineer2Manual::ctrlBPressing() { @@ -475,6 +521,8 @@ void Engineer2Manual::ctrlBRelease() } void Engineer2Manual::ctrlCPress() { + action_client_.cancelAllGoals(); + ROS_INFO("cancel all goal"); } void Engineer2Manual::ctrlDPress() { @@ -484,6 +532,11 @@ void Engineer2Manual::ctrlEPress() } void Engineer2Manual::ctrlFPress() { + prefix_ = "LV4_"; + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(EXCHANGE); } void Engineer2Manual::ctrlGPress() { @@ -493,9 +546,23 @@ void Engineer2Manual::ctrlQPress() } void Engineer2Manual::ctrlRPress() { + prefix_ = ""; + root_ = "CALIBRATION"; + servo_mode_ = JOINT; + calibration_gather_->reset(); + runStepQueue("CLOSE_ALL_GRIPPER"); + ROS_INFO_STREAM("START CALIBRATE"); + changeSpeedMode(NORMAL); + for (auto& stone : engineer_ui_.stone_num) + stone = false; } void Engineer2Manual::ctrlSPress() { + prefix_ = "BOTH_"; + root_ = "BIG_ISLAND"; + runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void Engineer2Manual::ctrlVPress() { @@ -505,6 +572,11 @@ void Engineer2Manual::ctrlVRelease() } void Engineer2Manual::ctrlWPress() { + prefix_ = "SIDE_"; + root_ = "BIG_ISLAND"; + runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void Engineer2Manual::ctrlXPress() { @@ -517,9 +589,13 @@ void Engineer2Manual::ctrlZPress() void Engineer2Manual::shiftPressing() { + changeSpeedMode(FAST); + ROS_INFO_ONCE("ENTER FAST SPEED MODE"); } void Engineer2Manual::shiftRelease() { + changeSpeedMode(NORMAL); + ROS_INFO_ONCE("EXIT FAST SPEED MODE"); } void Engineer2Manual::shiftBPress() { @@ -529,6 +605,18 @@ 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() { @@ -550,6 +638,16 @@ void Engineer2Manual::shiftRRelease() } void Engineer2Manual::shiftVPress() { + if (main_gripper_state_ == "close") + { + runStepQueue("OPEN_GRIPPER"); + engineer_ui_.gripper_state[0] = true; + } + else + { + runStepQueue("CLOSE_GRIPPER"); + engineer_ui_.gripper_state[0] = false; + } } void Engineer2Manual::shiftVRelease() { From aa4282eaf9f743a88ef0abda0ef364ef8133ed07 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Sat, 20 Jul 2024 03:48:11 +0800 Subject: [PATCH 21/42] Update CtrlF. --- src/engineer_manual.cpp | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 17daeeaa..c8a660d9 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -434,12 +434,33 @@ 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") + { + 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") + { + 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_) From 9b0f8816864bcb4b033af1fc54cc6d7960289b28 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 20 Jul 2024 21:23:52 +0800 Subject: [PATCH 22/42] Modify gpio state callback,arrange basic function keys. --- include/rm_manual/engineer2_manual.h | 9 +- src/engineer2_manual.cpp | 157 ++++++++++++++++++++------- 2 files changed, 127 insertions(+), 39 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 7b5f8750..7b626494 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -17,6 +17,7 @@ #include #include #include +#include "unordered_map" namespace rm_manual { @@ -142,11 +143,11 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool change_flag_{}; + bool change_flag_{}, has_ground_stone_{ 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_{}, main_gripper_state_{ "off" }, exchange_direction_{ "left" }; + std::string prefix_{}, root_{}, main_gripper_state_{ "off" }, exchange_direction_{ "left" }, exchange_level_{}; int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; @@ -170,6 +171,10 @@ class Engineer2Manual : public ChassisGimbalManual 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 }, + }; }; } // namespace rm_manual diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index e5b78f62..8c2f0ba5 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -100,11 +100,12 @@ void Engineer2Manual::run() { ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); - if (engineer_ui_ != old_ui_) - { - engineer_ui_pub_.publish(engineer_ui_); - old_ui_ = engineer_ui_; - } + // if (engineer_ui_ != old_ui_) + // { + // engineer_ui_pub_.publish(engineer_ui_); + // old_ui_ = engineer_ui_; + // } + engineer_ui_pub_.publish(engineer_ui_); } void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) @@ -203,8 +204,8 @@ void Engineer2Manual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) 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); + 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) @@ -217,26 +218,26 @@ void Engineer2Manual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { + auto it = stoneNumMap_.find(data->data); + if (it != stoneNumMap_.end()) + engineer_ui_.stone_num[it->second] = (data->data[0] == '+'); + if (engineer_ui_.stone_num[1] == false && engineer_ui_.stone_num[2] == false && engineer_ui_.stone_num[3] == false) + runStepQueue("CLOSE_SILVER_GRIPPER"); } void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { gpio_state_.gpio_state = data->gpio_state; - for (int i = 0; i < gpio_state_.gpio_state.size(); i++) + for (int i = 0; i <= 4; i++) + { + engineer_ui_.gripper_state[i] = data->gpio_state[i]; + } + engineer_ui_.gripper_state[5] = data->gpio_state[7]; + for (int i = 4; i >= 0; --i) { - if (gpio_state_.gpio_state[i] == true) - { - if (i == 0) - main_gripper_state_ = "open"; - engineer_ui_.gripper_state[i] = true; - } - else - { - if (i == 0) - main_gripper_state_ = "close"; - engineer_ui_.gripper_state[i] = false; - } + engineer_ui_.gripper_state[i] = data->gpio_state[i]; } + engineer_ui_.gripper_state[5] = data->gpio_state[7]; } void Engineer2Manual::sendCommand(const ros::Time& time) @@ -289,6 +290,20 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& initMode(); changeSpeedMode(NORMAL); } + if (!exchange_level_.empty()) + { + prefix_ = exchange_level_ + exchange_direction_; + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); + } + if (root_ == "EXCHANGE") + { + exchange_level_.clear(); + exchange_direction_.clear(); + has_ground_stone_ = false; + changeSpeedMode(EXCHANGE); + enterServo(); + } } void Engineer2Manual::enterServo() @@ -309,7 +324,7 @@ void Engineer2Manual::initMode() gimbal_mode_ = DIRECT; changeSpeedMode(NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; + chassis_cmd_sender_->getMsg()->command_source_frame = "base_link"; engineer_ui_.control_mode = "NORMAL"; } @@ -361,22 +376,20 @@ void Engineer2Manual::leftSwitchUpRise() { prefix_ = ""; root_ = "CALIBRATION"; + runStepQueue("MIDDLE_PITCH_UP"); + runStepQueue("CLOSE_ALL_GRIPPER"); calibration_gather_->reset(); for (auto& stone : engineer_ui_.stone_num) { stone = false; } - for (auto& gripper_state : engineer_ui_.gripper_state) - { - gripper_state = false; - } engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } void Engineer2Manual::leftSwitchUpFall() { - runStepQueue("HOME"); - runStepQueue("CLOSE_GRIPPER"); + runStepQueue("HOME_WITH_PITCH"); + runStepQueue("CLOSE_ALL_GRIPPER"); } void Engineer2Manual::leftSwitchDownRise() @@ -384,16 +397,15 @@ void Engineer2Manual::leftSwitchDownRise() if (main_gripper_state_ == "close") { runStepQueue("OPEN_MAIN_GRIPPER"); - engineer_ui_.gripper_state[0] = true; } else { runStepQueue("CLOSE_MAIN_GRIPPER"); - engineer_ui_.gripper_state[0] = false; } } void Engineer2Manual::leftSwitchDownFall() { + runStepQueue("MIDDLE_PITCH_UP"); } //--------------------- keyboard input ------------------------ @@ -510,7 +522,7 @@ void Engineer2Manual::ctrlBPress() { prefix_ = ""; root_ = "HOME"; - runStepQueue("HOME"); + runStepQueue(prefix_ + root_); changeSpeedMode(NORMAL); } void Engineer2Manual::ctrlBPressing() @@ -526,6 +538,10 @@ void Engineer2Manual::ctrlCPress() } void Engineer2Manual::ctrlDPress() { + prefix_ = "GRIPPER_"; + root_ = "THREE_SILVER"; + changeSpeedMode(LOW); + runStepQueue(prefix_ + root_); } void Engineer2Manual::ctrlEPress() { @@ -534,9 +550,9 @@ void Engineer2Manual::ctrlFPress() { prefix_ = "LV4_"; root_ = "EXCHANGE"; + changeSpeedMode(EXCHANGE); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(EXCHANGE); } void Engineer2Manual::ctrlGPress() { @@ -549,6 +565,7 @@ void Engineer2Manual::ctrlRPress() prefix_ = ""; root_ = "CALIBRATION"; servo_mode_ = JOINT; + has_ground_stone_ = false; calibration_gather_->reset(); runStepQueue("CLOSE_ALL_GRIPPER"); ROS_INFO_STREAM("START CALIBRATE"); @@ -560,9 +577,9 @@ void Engineer2Manual::ctrlSPress() { prefix_ = "BOTH_"; root_ = "BIG_ISLAND"; + changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void Engineer2Manual::ctrlVPress() { @@ -574,9 +591,9 @@ void Engineer2Manual::ctrlWPress() { prefix_ = "SIDE_"; root_ = "BIG_ISLAND"; + changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); - changeSpeedMode(LOW); } void Engineer2Manual::ctrlXPress() { @@ -620,15 +637,78 @@ void Engineer2Manual::shiftCPress() } void Engineer2Manual::shiftEPress() { + exchange_level_ = "LV5_"; + exchange_direction_ = "R_"; + if (!has_ground_stone_) + { + prefix_ = "GET_STORED_"; + if (engineer_ui_.stone_num[0]) + root_ = "GOLD"; + else if (engineer_ui_.stone_num[3]) + root_ = "SILVER3"; + else if (engineer_ui_.stone_num[2]) + root_ = "SILVER2"; + else if (engineer_ui_.stone_num[1]) + root_ = "SILVER1"; + runStepQueue(prefix_ + root_); + } + else + { + prefix_ = "LV5_R_"; + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); + } } void Engineer2Manual::shiftFPress() { } void Engineer2Manual::shiftGPress() { + exchange_level_ = "LV4_"; + exchange_direction_ = ""; + if (!has_ground_stone_) + { + prefix_ = "GET_STORED_"; + if (engineer_ui_.stone_num[0]) + root_ = "GOLD"; + else if (engineer_ui_.stone_num[3]) + root_ = "SILVER3"; + else if (engineer_ui_.stone_num[2]) + root_ = "SILVER2"; + else if (engineer_ui_.stone_num[1]) + root_ = "SILVER1"; + runStepQueue(prefix_ + root_); + } + else + { + prefix_ = "LV4_"; + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); + } } void Engineer2Manual::shiftQPress() { + exchange_level_ = "LV5_"; + exchange_direction_ = "L_"; + if (!has_ground_stone_) + { + prefix_ = "GET_STORED_"; + if (engineer_ui_.stone_num[0]) + root_ = "GOLD"; + else if (engineer_ui_.stone_num[3]) + root_ = "SILVER3"; + else if (engineer_ui_.stone_num[2]) + root_ = "SILVER2"; + else if (engineer_ui_.stone_num[1]) + root_ = "SILVER1"; + runStepQueue(prefix_ + root_); + } + else + { + prefix_ = "LV5_L_"; + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); + } } void Engineer2Manual::shiftRPress() { @@ -640,13 +720,11 @@ void Engineer2Manual::shiftVPress() { if (main_gripper_state_ == "close") { - runStepQueue("OPEN_GRIPPER"); - engineer_ui_.gripper_state[0] = true; + runStepQueue("OPEN_MAIN_GRIPPER"); } else { - runStepQueue("CLOSE_GRIPPER"); - engineer_ui_.gripper_state[0] = false; + runStepQueue("CLOSE_MAIN_GRIPPER"); } } void Engineer2Manual::shiftVRelease() @@ -654,6 +732,11 @@ void Engineer2Manual::shiftVRelease() } void Engineer2Manual::shiftXPress() { + has_ground_stone_ = true; + prefix_ = ""; + root_ = "GET_GROUND_STONE"; + changeSpeedMode(LOW); + runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZPress() { From 5ba7da39abd2fef086d9a555f305fdc01b407136 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 25 Jul 2024 06:38:07 +0800 Subject: [PATCH 23/42] Fix logic bug, fix wrong servo yaw command direction, adjust servo roll velocity, --- include/rm_manual/engineer2_manual.h | 4 +- src/engineer2_manual.cpp | 99 +++++++++++++--------------- 2 files changed, 46 insertions(+), 57 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 7b626494..c725eef0 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -143,11 +143,11 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool change_flag_{}, has_ground_stone_{ false }; + bool change_flag_{}, had_stone_in_hand_{ false }, main_gripper_on_{ 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_{}, main_gripper_state_{ "off" }, exchange_direction_{ "left" }, exchange_level_{}; + std::string prefix_{}, root_{}, exchange_direction_{ "left" }, exchange_level_{}; int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 8c2f0ba5..1a57b5c5 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -205,7 +205,7 @@ void Engineer2Manual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) 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); + 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) @@ -221,8 +221,6 @@ void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) auto it = stoneNumMap_.find(data->data); if (it != stoneNumMap_.end()) engineer_ui_.stone_num[it->second] = (data->data[0] == '+'); - if (engineer_ui_.stone_num[1] == false && engineer_ui_.stone_num[2] == false && engineer_ui_.stone_num[3] == false) - runStepQueue("CLOSE_SILVER_GRIPPER"); } void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) @@ -238,6 +236,7 @@ void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) engineer_ui_.gripper_state[i] = data->gpio_state[i]; } engineer_ui_.gripper_state[5] = data->gpio_state[7]; + main_gripper_on_ = data->gpio_state[0]; } void Engineer2Manual::sendCommand(const ros::Time& time) @@ -290,17 +289,9 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& initMode(); changeSpeedMode(NORMAL); } - if (!exchange_level_.empty()) + if (prefix_ == "LV4_" || prefix_ == "LV5_L_" || prefix_ == "LV5_R_") { - prefix_ = exchange_level_ + exchange_direction_; - root_ = "EXCHANGE"; - runStepQueue(prefix_ + root_); - } - if (root_ == "EXCHANGE") - { - exchange_level_.clear(); - exchange_direction_.clear(); - has_ground_stone_ = false; + had_stone_in_hand_ = false; changeSpeedMode(EXCHANGE); enterServo(); } @@ -314,7 +305,7 @@ void Engineer2Manual::enterServo() servo_reset_caller_->callService(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); action_client_.cancelAllGoals(); - chassis_cmd_sender_->getMsg()->command_source_frame = "link3"; + chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; engineer_ui_.control_mode = "SERVO"; } @@ -324,7 +315,7 @@ void Engineer2Manual::initMode() gimbal_mode_ = DIRECT; changeSpeedMode(NORMAL); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - chassis_cmd_sender_->getMsg()->command_source_frame = "base_link"; + chassis_cmd_sender_->getMsg()->command_source_frame = "yaw"; engineer_ui_.control_mode = "NORMAL"; } @@ -394,7 +385,7 @@ void Engineer2Manual::leftSwitchUpFall() void Engineer2Manual::leftSwitchDownRise() { - if (main_gripper_state_ == "close") + if (!main_gripper_on_) { runStepQueue("OPEN_MAIN_GRIPPER"); } @@ -436,7 +427,7 @@ void Engineer2Manual::bRelease() } void Engineer2Manual::cPressing() { - angular_z_scale_ = 0.6; + angular_z_scale_ = -0.8; } void Engineer2Manual::cRelease() { @@ -488,22 +479,22 @@ void Engineer2Manual::xPress() switch (gimbal_direction_) { case 0: - runStepQueue("GIMBAL_RIGHT"); + runStepQueue("GIMBAL_R"); gimbal_direction_ = 1; break; case 1: - runStepQueue("GIMBAL_LEFT"); + runStepQueue("GIMBAL_L"); gimbal_direction_ = 2; break; case 2: - runStepQueue("GIMBAL_FRONT"); + runStepQueue("GIMBAL_F"); gimbal_direction_ = 0; break; } } void Engineer2Manual::zPressing() { - angular_z_scale_ = -0.6; + angular_z_scale_ = 0.8; } void Engineer2Manual::zRelease() { @@ -556,16 +547,21 @@ void Engineer2Manual::ctrlFPress() } void Engineer2Manual::ctrlGPress() { + prefix_ = ""; + root_ = "MID_BIG_ISLAND"; + changeSpeedMode(LOW); + runStepQueue(prefix_ + root_); } void Engineer2Manual::ctrlQPress() { } void Engineer2Manual::ctrlRPress() { + runStepQueue("MIDDLE_PITCH_UP"); prefix_ = ""; root_ = "CALIBRATION"; servo_mode_ = JOINT; - has_ground_stone_ = false; + had_stone_in_hand_ = false; calibration_gather_->reset(); runStepQueue("CLOSE_ALL_GRIPPER"); ROS_INFO_STREAM("START CALIBRATE"); @@ -577,6 +573,7 @@ void Engineer2Manual::ctrlSPress() { prefix_ = "BOTH_"; root_ = "BIG_ISLAND"; + had_stone_in_hand_ = true; changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -591,6 +588,7 @@ void Engineer2Manual::ctrlWPress() { prefix_ = "SIDE_"; root_ = "BIG_ISLAND"; + had_stone_in_hand_ = true; changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -607,12 +605,10 @@ void Engineer2Manual::ctrlZPress() void Engineer2Manual::shiftPressing() { changeSpeedMode(FAST); - ROS_INFO_ONCE("ENTER FAST SPEED MODE"); } void Engineer2Manual::shiftRelease() { changeSpeedMode(NORMAL); - ROS_INFO_ONCE("EXIT FAST SPEED MODE"); } void Engineer2Manual::shiftBPress() { @@ -637,24 +633,21 @@ void Engineer2Manual::shiftCPress() } void Engineer2Manual::shiftEPress() { - exchange_level_ = "LV5_"; - exchange_direction_ = "R_"; - if (!has_ground_stone_) + prefix_ = "LV5_R_"; + if (!had_stone_in_hand_) { - prefix_ = "GET_STORED_"; if (engineer_ui_.stone_num[0]) - root_ = "GOLD"; + root_ = "G"; else if (engineer_ui_.stone_num[3]) - root_ = "SILVER3"; + root_ = "S3"; else if (engineer_ui_.stone_num[2]) - root_ = "SILVER2"; + root_ = "S2"; else if (engineer_ui_.stone_num[1]) - root_ = "SILVER1"; + root_ = "S1"; runStepQueue(prefix_ + root_); } else { - prefix_ = "LV5_R_"; root_ = "EXCHANGE"; runStepQueue(prefix_ + root_); } @@ -664,48 +657,42 @@ void Engineer2Manual::shiftFPress() } void Engineer2Manual::shiftGPress() { - exchange_level_ = "LV4_"; - exchange_direction_ = ""; - if (!has_ground_stone_) + prefix_ = "LV4_"; + if (!had_stone_in_hand_) { - prefix_ = "GET_STORED_"; if (engineer_ui_.stone_num[0]) - root_ = "GOLD"; + root_ = "G"; else if (engineer_ui_.stone_num[3]) - root_ = "SILVER3"; + root_ = "S3"; else if (engineer_ui_.stone_num[2]) - root_ = "SILVER2"; + root_ = "S2"; else if (engineer_ui_.stone_num[1]) - root_ = "SILVER1"; + root_ = "S1"; runStepQueue(prefix_ + root_); } else { - prefix_ = "LV4_"; root_ = "EXCHANGE"; runStepQueue(prefix_ + root_); } } void Engineer2Manual::shiftQPress() { - exchange_level_ = "LV5_"; - exchange_direction_ = "L_"; - if (!has_ground_stone_) + prefix_ = "LV5_L_"; + if (!had_stone_in_hand_) { - prefix_ = "GET_STORED_"; if (engineer_ui_.stone_num[0]) - root_ = "GOLD"; + root_ = "G"; else if (engineer_ui_.stone_num[3]) - root_ = "SILVER3"; + root_ = "S3"; else if (engineer_ui_.stone_num[2]) - root_ = "SILVER2"; + root_ = "S2"; else if (engineer_ui_.stone_num[1]) - root_ = "SILVER1"; + root_ = "S1"; runStepQueue(prefix_ + root_); } else { - prefix_ = "LV5_L_"; root_ = "EXCHANGE"; runStepQueue(prefix_ + root_); } @@ -718,21 +705,23 @@ void Engineer2Manual::shiftRRelease() } void Engineer2Manual::shiftVPress() { - if (main_gripper_state_ == "close") + prefix_ = ""; + if (!main_gripper_on_) { - runStepQueue("OPEN_MAIN_GRIPPER"); + root_ = "OM"; } else { - runStepQueue("CLOSE_MAIN_GRIPPER"); + root_ = "CM"; } + runStepQueue(root_); } void Engineer2Manual::shiftVRelease() { } void Engineer2Manual::shiftXPress() { - has_ground_stone_ = true; + had_stone_in_hand_ = true; prefix_ = ""; root_ = "GET_GROUND_STONE"; changeSpeedMode(LOW); From 010a443da30e052cf84922a7ff6bdd0396f29c9d Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Sat, 27 Jul 2024 00:44:10 +0800 Subject: [PATCH 24/42] Add change exchange direction, move back to exchange pos and manual reduce stone num. --- include/rm_manual/engineer2_manual.h | 5 +-- src/engineer2_manual.cpp | 53 ++++++++++++++++++++++++++-- 2 files changed, 53 insertions(+), 5 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index c725eef0..bb854cfd 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -115,6 +115,7 @@ class Engineer2Manual : public ChassisGimbalManual void qPressing(); void qRelease(); void rPress(); + void rRelease(); void vPressing(); void vRelease(); void xPress(); @@ -143,11 +144,11 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool change_flag_{}, had_stone_in_hand_{ false }, main_gripper_on_{ false }; + bool mouse_left_pressed_{}, had_stone_in_hand_{ false }, main_gripper_on_{ false }, stone_reduced_{ 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_{}, exchange_direction_{ "left" }, exchange_level_{}; + std::string prefix_{}, root_{}, exchange_direction_{ "left" }; int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 1a57b5c5..8e0b8f99 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -70,6 +70,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere 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)); @@ -168,6 +169,7 @@ void Engineer2Manual::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); @@ -281,7 +283,7 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& ROS_INFO("Finished in state [%s]", state.toString().c_str()); ROS_INFO("Result: %i", result->finish); ROS_INFO("Done %s", (prefix_ + root_).c_str()); - change_flag_ = true; + mouse_left_pressed_ = true; ROS_INFO("%i", result->finish); operating_mode_ = MANUAL; if (root_ == "HOME") @@ -403,10 +405,10 @@ void Engineer2Manual::leftSwitchDownFall() // mouse input void Engineer2Manual::mouseLeftRelease() { - if (change_flag_) + if (mouse_left_pressed_) { root_ += "0"; - change_flag_ = false; + mouse_left_pressed_ = false; runStepQueue(prefix_ + root_); ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } @@ -467,7 +469,27 @@ void Engineer2Manual::qRelease() } void Engineer2Manual::rPress() { + // if (!stone_reduced_) + // { + if (had_stone_in_hand_) + had_stone_in_hand_ = false; + else if (engineer_ui_.stone_num[0]) + engineer_ui_.stone_num[0] = false; + else if (engineer_ui_.stone_num[3]) + engineer_ui_.stone_num[3] = false; + else if (engineer_ui_.stone_num[2]) + engineer_ui_.stone_num[2] = false; + else if (engineer_ui_.stone_num[1]) + engineer_ui_.stone_num[1] = false; + stone_reduced_ = true; + // } } + +void Engineer2Manual::rRelease() +{ + stone_reduced_ = false; +} + void Engineer2Manual::vPressing() { } @@ -633,6 +655,7 @@ void Engineer2Manual::shiftCPress() } void Engineer2Manual::shiftEPress() { + exchange_direction_ = "right"; prefix_ = "LV5_R_"; if (!had_stone_in_hand_) { @@ -654,6 +677,13 @@ void Engineer2Manual::shiftEPress() } void Engineer2Manual::shiftFPress() { + if (exchange_direction_ == "left") + prefix_ = "LV5_L_"; + else + prefix_ = "LV5_R_"; + + root_ = "EXCHANGE"; + runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftGPress() { @@ -675,9 +705,11 @@ void Engineer2Manual::shiftGPress() root_ = "EXCHANGE"; runStepQueue(prefix_ + root_); } + ROS_INFO_STREAM(prefix_ + root_); } void Engineer2Manual::shiftQPress() { + exchange_direction_ = "left"; prefix_ = "LV5_L_"; if (!had_stone_in_hand_) { @@ -696,6 +728,7 @@ void Engineer2Manual::shiftQPress() root_ = "EXCHANGE"; runStepQueue(prefix_ + root_); } + ROS_INFO_STREAM(prefix_ + root_); } void Engineer2Manual::shiftRPress() { @@ -725,10 +758,24 @@ void Engineer2Manual::shiftXPress() prefix_ = ""; root_ = "GET_GROUND_STONE"; changeSpeedMode(LOW); + ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZPress() { + if (exchange_direction_ == "left") + { + prefix_ = "LV5_R_"; + exchange_direction_ = "right"; + } + else if (exchange_direction_ == "right") + { + prefix_ = "LV5_L_"; + exchange_direction_ = "left"; + } + root_ = "EXCHANGE"; + ROS_INFO_STREAM(prefix_ + root_); + runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZRelease() { From 9ae451365c0fba1706239c1f773b8e237b9dd182 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 29 Jul 2024 20:49:02 +0800 Subject: [PATCH 25/42] Fix error, add multi state ui. --- include/rm_manual/engineer2_manual.h | 7 +++++++ src/engineer2_manual.cpp | 18 +++++++++++------- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index bb854cfd..7a5a848d 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -176,6 +176,13 @@ class Engineer2Manual : public ChassisGimbalManual 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/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 8e0b8f99..d603a09c 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -26,7 +26,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere 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.3); + 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("fast_gyro_scale", fast_gyro_scale_, 0.5); chassis_nh.param("normal_gyro_scale", normal_gyro_scale_, 0.15); @@ -285,6 +285,8 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& ROS_INFO("Done %s", (prefix_ + root_).c_str()); mouse_left_pressed_ = true; ROS_INFO("%i", result->finish); + engineer_ui_.symbol = UiState::NONE; + operating_mode_ = MANUAL; if (root_ == "HOME") { @@ -369,8 +371,7 @@ void Engineer2Manual::leftSwitchUpRise() { prefix_ = ""; root_ = "CALIBRATION"; - runStepQueue("MIDDLE_PITCH_UP"); - runStepQueue("CLOSE_ALL_GRIPPER"); + runStepQueue("CALIBRATION"); calibration_gather_->reset(); for (auto& stone : engineer_ui_.stone_num) { @@ -382,7 +383,6 @@ void Engineer2Manual::leftSwitchUpRise() void Engineer2Manual::leftSwitchUpFall() { runStepQueue("HOME_WITH_PITCH"); - runStepQueue("CLOSE_ALL_GRIPPER"); } void Engineer2Manual::leftSwitchDownRise() @@ -551,6 +551,7 @@ void Engineer2Manual::ctrlCPress() } void Engineer2Manual::ctrlDPress() { + engineer_ui_.symbol = UiState::SMALL_ISLAND; prefix_ = "GRIPPER_"; root_ = "THREE_SILVER"; changeSpeedMode(LOW); @@ -569,6 +570,7 @@ void Engineer2Manual::ctrlFPress() } void Engineer2Manual::ctrlGPress() { + engineer_ui_.symbol = UiState::BIG_ISLAND; prefix_ = ""; root_ = "MID_BIG_ISLAND"; changeSpeedMode(LOW); @@ -579,20 +581,21 @@ void Engineer2Manual::ctrlQPress() } void Engineer2Manual::ctrlRPress() { - runStepQueue("MIDDLE_PITCH_UP"); + runStepQueue("CALIBRATION"); prefix_ = ""; root_ = "CALIBRATION"; servo_mode_ = JOINT; had_stone_in_hand_ = false; calibration_gather_->reset(); - runStepQueue("CLOSE_ALL_GRIPPER"); ROS_INFO_STREAM("START CALIBRATE"); changeSpeedMode(NORMAL); for (auto& stone : engineer_ui_.stone_num) stone = false; + runStepQueue("CA"); } void Engineer2Manual::ctrlSPress() { + engineer_ui_.symbol = UiState::BIG_ISLAND; prefix_ = "BOTH_"; root_ = "BIG_ISLAND"; had_stone_in_hand_ = true; @@ -608,6 +611,7 @@ void Engineer2Manual::ctrlVRelease() } void Engineer2Manual::ctrlWPress() { + engineer_ui_.symbol = UiState::BIG_ISLAND; prefix_ = "SIDE_"; root_ = "BIG_ISLAND"; had_stone_in_hand_ = true; @@ -756,7 +760,7 @@ void Engineer2Manual::shiftXPress() { had_stone_in_hand_ = true; prefix_ = ""; - root_ = "GET_GROUND_STONE"; + root_ = "GROUND_STONE"; changeSpeedMode(LOW); ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); From a33f82d54c727dd87ee5f5aba75fa102c010dea8 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Tue, 30 Jul 2024 06:38:46 +0800 Subject: [PATCH 26/42] Add function to exchange side gold stone at front of car. --- include/rm_manual/engineer2_manual.h | 4 +- src/engineer2_manual.cpp | 106 +++++++++++++++++---------- 2 files changed, 68 insertions(+), 42 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 7a5a848d..c68065df 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -144,11 +144,11 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool mouse_left_pressed_{}, had_stone_in_hand_{ false }, main_gripper_on_{ false }, stone_reduced_{ false }; + bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ 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_{}, exchange_direction_{ "left" }; + std::string prefix_{}, root_{}, exchange_direction_{ "left" }, exchange_arm_position_{ "normal" }; int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index d603a09c..f2cf8ab2 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -10,7 +10,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) { - engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); + engineer_ui_pub_ = nh.advertise("/engineer2_ui", 10); ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); @@ -295,7 +295,8 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& } if (prefix_ == "LV4_" || prefix_ == "LV5_L_" || prefix_ == "LV5_R_") { - had_stone_in_hand_ = false; + had_ground_stone_ = false; + had_side_gold_ = false; changeSpeedMode(EXCHANGE); enterServo(); } @@ -372,6 +373,8 @@ void Engineer2Manual::leftSwitchUpRise() prefix_ = ""; root_ = "CALIBRATION"; runStepQueue("CALIBRATION"); + had_side_gold_ = false; + had_ground_stone_ = false; calibration_gather_->reset(); for (auto& stone : engineer_ui_.stone_num) { @@ -469,10 +472,10 @@ void Engineer2Manual::qRelease() } void Engineer2Manual::rPress() { - // if (!stone_reduced_) - // { - if (had_stone_in_hand_) - had_stone_in_hand_ = false; + if (had_side_gold_) + had_side_gold_ = false; + if (had_ground_stone_) + had_ground_stone_ = false; else if (engineer_ui_.stone_num[0]) engineer_ui_.stone_num[0] = false; else if (engineer_ui_.stone_num[3]) @@ -481,13 +484,10 @@ void Engineer2Manual::rPress() engineer_ui_.stone_num[2] = false; else if (engineer_ui_.stone_num[1]) engineer_ui_.stone_num[1] = false; - stone_reduced_ = true; - // } } void Engineer2Manual::rRelease() { - stone_reduced_ = false; } void Engineer2Manual::vPressing() @@ -585,7 +585,8 @@ void Engineer2Manual::ctrlRPress() prefix_ = ""; root_ = "CALIBRATION"; servo_mode_ = JOINT; - had_stone_in_hand_ = false; + had_ground_stone_ = false; + had_side_gold_ = false; calibration_gather_->reset(); ROS_INFO_STREAM("START CALIBRATE"); changeSpeedMode(NORMAL); @@ -598,7 +599,8 @@ void Engineer2Manual::ctrlSPress() engineer_ui_.symbol = UiState::BIG_ISLAND; prefix_ = "BOTH_"; root_ = "BIG_ISLAND"; - had_stone_in_hand_ = true; + had_side_gold_ = true; + had_ground_stone_ = false; changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -614,7 +616,8 @@ void Engineer2Manual::ctrlWPress() engineer_ui_.symbol = UiState::BIG_ISLAND; prefix_ = "SIDE_"; root_ = "BIG_ISLAND"; - had_stone_in_hand_ = true; + had_side_gold_ = true; + had_ground_stone_ = false; changeSpeedMode(LOW); runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); @@ -661,8 +664,19 @@ void Engineer2Manual::shiftEPress() { exchange_direction_ = "right"; prefix_ = "LV5_R_"; - if (!had_stone_in_hand_) + if (had_ground_stone_) + { + exchange_arm_position_ = "normal"; + root_ = "EXCHANGE"; + } + else if (had_side_gold_) + { + exchange_arm_position_ = "front"; + root_ = "CAR_FRONT_EXCHANGE"; + } + else { + exchange_arm_position_ = "normal"; if (engineer_ui_.stone_num[0]) root_ = "G"; else if (engineer_ui_.stone_num[3]) @@ -671,13 +685,8 @@ void Engineer2Manual::shiftEPress() root_ = "S2"; else if (engineer_ui_.stone_num[1]) root_ = "S1"; - runStepQueue(prefix_ + root_); - } - else - { - root_ = "EXCHANGE"; - runStepQueue(prefix_ + root_); } + runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftFPress() { @@ -685,15 +694,28 @@ void Engineer2Manual::shiftFPress() prefix_ = "LV5_L_"; else prefix_ = "LV5_R_"; - - root_ = "EXCHANGE"; + if (exchange_arm_position_ == "normal") + root_ = "EXCHANGE"; + else + root_ = "CAR_FRONT_EXCHANGE"; runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftGPress() { prefix_ = "LV4_"; - if (!had_stone_in_hand_) + if (had_ground_stone_) { + exchange_arm_position_ = "normal"; + root_ = "EXCHANGE"; + } + else if (had_side_gold_) + { + exchange_arm_position_ = "front"; + root_ = "CAR_FRONT_EXCHANGE"; + } + else + { + exchange_arm_position_ = "normal"; if (engineer_ui_.stone_num[0]) root_ = "G"; else if (engineer_ui_.stone_num[3]) @@ -702,21 +724,27 @@ void Engineer2Manual::shiftGPress() root_ = "S2"; else if (engineer_ui_.stone_num[1]) root_ = "S1"; - runStepQueue(prefix_ + root_); - } - else - { - root_ = "EXCHANGE"; - runStepQueue(prefix_ + root_); } + runStepQueue(prefix_ + root_); ROS_INFO_STREAM(prefix_ + root_); } void Engineer2Manual::shiftQPress() { exchange_direction_ = "left"; prefix_ = "LV5_L_"; - if (!had_stone_in_hand_) + if (had_ground_stone_) { + exchange_arm_position_ = "normal"; + root_ = "EXCHANGE"; + } + else if (had_side_gold_) + { + exchange_arm_position_ = "front"; + root_ = "CAR_FRONT_EXCHANGE"; + } + else + { + exchange_arm_position_ = "normal"; if (engineer_ui_.stone_num[0]) root_ = "G"; else if (engineer_ui_.stone_num[3]) @@ -725,13 +753,8 @@ void Engineer2Manual::shiftQPress() root_ = "S2"; else if (engineer_ui_.stone_num[1]) root_ = "S1"; - runStepQueue(prefix_ + root_); - } - else - { - root_ = "EXCHANGE"; - runStepQueue(prefix_ + root_); } + runStepQueue(prefix_ + root_); ROS_INFO_STREAM(prefix_ + root_); } void Engineer2Manual::shiftRPress() @@ -743,13 +766,13 @@ void Engineer2Manual::shiftRRelease() void Engineer2Manual::shiftVPress() { prefix_ = ""; - if (!main_gripper_on_) + if (main_gripper_on_) { - root_ = "OM"; + root_ = "CM"; } else { - root_ = "CM"; + root_ = "OM"; } runStepQueue(root_); } @@ -758,7 +781,7 @@ void Engineer2Manual::shiftVRelease() } void Engineer2Manual::shiftXPress() { - had_stone_in_hand_ = true; + had_ground_stone_ = true; prefix_ = ""; root_ = "GROUND_STONE"; changeSpeedMode(LOW); @@ -777,7 +800,10 @@ void Engineer2Manual::shiftZPress() prefix_ = "LV5_L_"; exchange_direction_ = "left"; } - root_ = "EXCHANGE"; + if (exchange_arm_position_ == "normal") + root_ = "EXCHANGE"; + else + root_ = "CAR_FRONT_EXCHANGE"; ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } From f22ef528f8154cc1794052d802c142ff59c79c9f Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Wed, 31 Jul 2024 08:24:47 +0800 Subject: [PATCH 27/42] Change chassis command source frame to yaw when using PC. --- src/engineer2_manual.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index f2cf8ab2..156ad6aa 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -200,6 +200,7 @@ 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_); } From 2801ffac726d4e8507a0c0ce6223dca99bbf3a78 Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Wed, 31 Jul 2024 22:28:40 +0800 Subject: [PATCH 28/42] Change servo frame and delete joint2 calibration. --- src/engineer_manual.cpp | 374 +++++++++++++++++++++++++++++++++------- 1 file changed, 316 insertions(+), 58 deletions(-) diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index c8a660d9..c9509e95 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,21 @@ 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 +227,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 +247,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 +297,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 +334,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 +343,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 +351,7 @@ void EngineerManual::remoteControlTurnOff() ManualBase::remoteControlTurnOff(); action_client_.cancelAllGoals(); } + void EngineerManual::gimbalOutputOn() { ChassisGimbalManual::gimbalOutputOn(); @@ -345,11 +396,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 +415,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 +437,39 @@ 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; - } - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME" << stone_num_); + prefix_ = ""; + root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; + runStepQueue(prefix_ + root_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); + } void EngineerManual::ctrlCPress() @@ -426,6 +484,7 @@ void EngineerManual::ctrlDPress() root_ = "SMALL_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlEPress() @@ -434,7 +493,7 @@ void EngineerManual::ctrlEPress() void EngineerManual::ctrlFPress() { - if(root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT") + if(root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") { prefix_ = ""; root_ = "RIGHT_EXCHANGE"; @@ -442,7 +501,7 @@ void EngineerManual::ctrlFPress() ROS_INFO("%s", (prefix_ + root_).c_str()); changeSpeedMode(EXCHANGE); } - else if(root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT") + else if(root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") { prefix_ = ""; root_ = "LEFT_EXCHANGE"; @@ -460,22 +519,21 @@ void EngineerManual::ctrlFPress() } } - 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"); } @@ -492,6 +550,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() @@ -500,6 +563,7 @@ void EngineerManual::ctrlSPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlVPress() @@ -515,6 +579,7 @@ void EngineerManual::ctrlWPress() root_ = "BIG_ISLAND"; runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); + changeSpeedMode(LOW); } void EngineerManual::ctrlXPress() @@ -535,7 +600,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() { @@ -577,6 +642,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() { @@ -595,28 +666,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() { @@ -657,23 +759,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!!"; + 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"); + } + 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!!"; - stone_num_ = 0; break; case 1: - root_ = "GET_DOWN_STONE_BIN"; + 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: - root_ = "GET_UP_STONE_BIN"; + 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_); @@ -694,10 +928,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() @@ -710,6 +946,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 From 0c3d0a58bf1d984d596ca429cc595657d375daff Mon Sep 17 00:00:00 2001 From: 2194555 <3631676002@qq.com> Date: Wed, 31 Jul 2024 22:29:42 +0800 Subject: [PATCH 29/42] Change servo frame and delete joint2 calibration. --- include/rm_manual/engineer_manual.h | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 17e4c589..272a2f38 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,44 @@ 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_; From f937240f796f8b954bdb6fc25438362b34fd0c94 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 01:11:36 +0800 Subject: [PATCH 30/42] Send gripper ui independently. --- include/rm_manual/engineer2_manual.h | 11 +-- src/engineer2_manual.cpp | 102 +++++++++++++-------------- 2 files changed, 55 insertions(+), 58 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index c68065df..958f427d 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -15,7 +15,8 @@ #include #include #include -#include +#include +#include #include #include "unordered_map" @@ -144,7 +145,8 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ false }; + bool mouse_left_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_{}; @@ -155,10 +157,11 @@ class Engineer2Manual : public ChassisGimbalManual ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; - ros::Publisher engineer_ui_pub_; + ros::Publisher engineer_ui_pub_, gripper_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::Engineer2Ui engineer_ui_, old_ui_; + rm_msgs::EngineerUi engineer_ui_, old_ui_; + rm_msgs::VisualizeStateData gripper_state_{}; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 156ad6aa..4f113217 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -10,14 +10,15 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) { - engineer_ui_pub_ = nh.advertise("/engineer2_ui", 10); + 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", 1); + gripper_state_.state.assign(5, 0); // Servo ros::NodeHandle nh_servo(nh, "servo"); servo_command_sender_ = new rm_common::Vel3DCommandSender(nh_servo); @@ -107,6 +108,7 @@ void Engineer2Manual::run() // old_ui_ = engineer_ui_; // } engineer_ui_pub_.publish(engineer_ui_); + gripper_ui_pub_.publish(gripper_state_); } void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) @@ -223,7 +225,8 @@ void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { auto it = stoneNumMap_.find(data->data); if (it != stoneNumMap_.end()) - engineer_ui_.stone_num[it->second] = (data->data[0] == '+'); + stone_state_[it->second] = (data->data[0] == '+'); + // stone_state: gold, silver1, silver2, silver3 } void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) @@ -231,14 +234,10 @@ void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) gpio_state_.gpio_state = data->gpio_state; for (int i = 0; i <= 4; i++) { - engineer_ui_.gripper_state[i] = data->gpio_state[i]; - } - engineer_ui_.gripper_state[5] = data->gpio_state[7]; - for (int i = 4; i >= 0; --i) - { - engineer_ui_.gripper_state[i] = data->gpio_state[i]; + gripper_state_.state[i] = data->gpio_state[i]; + gripper_state_.state[4 - i] = data->gpio_state[4 - i]; } - engineer_ui_.gripper_state[5] = data->gpio_state[7]; + // gripper state: main, silver1, silver2, silver3, gold, silver pump main_gripper_on_ = data->gpio_state[0]; } @@ -377,7 +376,7 @@ void Engineer2Manual::leftSwitchUpRise() had_side_gold_ = false; had_ground_stone_ = false; calibration_gather_->reset(); - for (auto& stone : engineer_ui_.stone_num) + for (auto& stone : stone_state_) { stone = false; } @@ -477,14 +476,14 @@ void Engineer2Manual::rPress() had_side_gold_ = false; if (had_ground_stone_) had_ground_stone_ = false; - else if (engineer_ui_.stone_num[0]) - engineer_ui_.stone_num[0] = false; - else if (engineer_ui_.stone_num[3]) - engineer_ui_.stone_num[3] = false; - else if (engineer_ui_.stone_num[2]) - engineer_ui_.stone_num[2] = false; - else if (engineer_ui_.stone_num[1]) - engineer_ui_.stone_num[1] = 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() @@ -563,11 +562,15 @@ void Engineer2Manual::ctrlEPress() } void Engineer2Manual::ctrlFPress() { - prefix_ = "LV4_"; - root_ = "EXCHANGE"; - changeSpeedMode(EXCHANGE); + if (exchange_direction_ == "left") + prefix_ = "LV5_L_"; + else + prefix_ = "LV5_R_"; + if (exchange_arm_position_ == "normal") + root_ = "EXCHANGE"; + else + root_ = "CAR_FRONT_EXCHANGE"; runStepQueue(prefix_ + root_); - ROS_INFO("%s", (prefix_ + root_).c_str()); } void Engineer2Manual::ctrlGPress() { @@ -591,7 +594,7 @@ void Engineer2Manual::ctrlRPress() calibration_gather_->reset(); ROS_INFO_STREAM("START CALIBRATE"); changeSpeedMode(NORMAL); - for (auto& stone : engineer_ui_.stone_num) + for (auto& stone : stone_state_) stone = false; runStepQueue("CA"); } @@ -678,13 +681,13 @@ void Engineer2Manual::shiftEPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -692,13 +695,20 @@ void Engineer2Manual::shiftEPress() void Engineer2Manual::shiftFPress() { if (exchange_direction_ == "left") - prefix_ = "LV5_L_"; - else + { prefix_ = "LV5_R_"; + exchange_direction_ = "right"; + } + else if (exchange_direction_ == "right") + { + prefix_ = "LV5_L_"; + exchange_direction_ = "left"; + } if (exchange_arm_position_ == "normal") root_ = "EXCHANGE"; else root_ = "CAR_FRONT_EXCHANGE"; + ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftGPress() @@ -717,13 +727,13 @@ void Engineer2Manual::shiftGPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -746,13 +756,13 @@ void Engineer2Manual::shiftQPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -791,22 +801,6 @@ void Engineer2Manual::shiftXPress() } void Engineer2Manual::shiftZPress() { - if (exchange_direction_ == "left") - { - prefix_ = "LV5_R_"; - exchange_direction_ = "right"; - } - else if (exchange_direction_ == "right") - { - prefix_ = "LV5_L_"; - exchange_direction_ = "left"; - } - if (exchange_arm_position_ == "normal") - root_ = "EXCHANGE"; - else - root_ = "CAR_FRONT_EXCHANGE"; - ROS_INFO_STREAM(prefix_ + root_); - runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZRelease() { From 2647b5cba2d9c5b88ff585921d3735f43fcc13fe Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 04:30:00 +0800 Subject: [PATCH 31/42] Revert "Send gripper ui independently." This reverts commit f937240f796f8b954bdb6fc25438362b34fd0c94. --- include/rm_manual/engineer2_manual.h | 11 ++- src/engineer2_manual.cpp | 102 ++++++++++++++------------- 2 files changed, 58 insertions(+), 55 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 958f427d..c68065df 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -15,8 +15,7 @@ #include #include #include -#include -#include +#include #include #include "unordered_map" @@ -145,8 +144,7 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ false }, - stone_state_[4]{}; + bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ 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_{}; @@ -157,11 +155,10 @@ class Engineer2Manual : public ChassisGimbalManual ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; - ros::Publisher engineer_ui_pub_, gripper_ui_pub_; + ros::Publisher engineer_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::EngineerUi engineer_ui_, old_ui_; - rm_msgs::VisualizeStateData gripper_state_{}; + rm_msgs::Engineer2Ui engineer_ui_, old_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 4f113217..156ad6aa 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -10,15 +10,14 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) { - engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); + engineer_ui_pub_ = nh.advertise("/engineer2_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", 1); - gripper_state_.state.assign(5, 0); + // Servo ros::NodeHandle nh_servo(nh, "servo"); servo_command_sender_ = new rm_common::Vel3DCommandSender(nh_servo); @@ -108,7 +107,6 @@ void Engineer2Manual::run() // old_ui_ = engineer_ui_; // } engineer_ui_pub_.publish(engineer_ui_); - gripper_ui_pub_.publish(gripper_state_); } void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) @@ -225,8 +223,7 @@ 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] == '+'); - // stone_state: gold, silver1, silver2, silver3 + engineer_ui_.stone_num[it->second] = (data->data[0] == '+'); } void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) @@ -234,10 +231,14 @@ void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) gpio_state_.gpio_state = data->gpio_state; for (int i = 0; i <= 4; i++) { - gripper_state_.state[i] = data->gpio_state[i]; - gripper_state_.state[4 - i] = data->gpio_state[4 - i]; + engineer_ui_.gripper_state[i] = data->gpio_state[i]; + } + engineer_ui_.gripper_state[5] = data->gpio_state[7]; + for (int i = 4; i >= 0; --i) + { + engineer_ui_.gripper_state[i] = data->gpio_state[i]; } - // gripper state: main, silver1, silver2, silver3, gold, silver pump + engineer_ui_.gripper_state[5] = data->gpio_state[7]; main_gripper_on_ = data->gpio_state[0]; } @@ -376,7 +377,7 @@ void Engineer2Manual::leftSwitchUpRise() had_side_gold_ = false; had_ground_stone_ = false; calibration_gather_->reset(); - for (auto& stone : stone_state_) + for (auto& stone : engineer_ui_.stone_num) { stone = false; } @@ -476,14 +477,14 @@ void Engineer2Manual::rPress() 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; + else if (engineer_ui_.stone_num[0]) + engineer_ui_.stone_num[0] = false; + else if (engineer_ui_.stone_num[3]) + engineer_ui_.stone_num[3] = false; + else if (engineer_ui_.stone_num[2]) + engineer_ui_.stone_num[2] = false; + else if (engineer_ui_.stone_num[1]) + engineer_ui_.stone_num[1] = false; } void Engineer2Manual::rRelease() @@ -562,15 +563,11 @@ 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_ = "CAR_FRONT_EXCHANGE"; + prefix_ = "LV4_"; + root_ = "EXCHANGE"; + changeSpeedMode(EXCHANGE); runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); } void Engineer2Manual::ctrlGPress() { @@ -594,7 +591,7 @@ void Engineer2Manual::ctrlRPress() calibration_gather_->reset(); ROS_INFO_STREAM("START CALIBRATE"); changeSpeedMode(NORMAL); - for (auto& stone : stone_state_) + for (auto& stone : engineer_ui_.stone_num) stone = false; runStepQueue("CA"); } @@ -681,13 +678,13 @@ void Engineer2Manual::shiftEPress() else { exchange_arm_position_ = "normal"; - if (stone_state_[0]) + if (engineer_ui_.stone_num[0]) root_ = "G"; - else if (stone_state_[3]) + else if (engineer_ui_.stone_num[3]) root_ = "S3"; - else if (stone_state_[2]) + else if (engineer_ui_.stone_num[2]) root_ = "S2"; - else if (stone_state_[1]) + else if (engineer_ui_.stone_num[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -695,20 +692,13 @@ void Engineer2Manual::shiftEPress() void Engineer2Manual::shiftFPress() { if (exchange_direction_ == "left") - { - prefix_ = "LV5_R_"; - exchange_direction_ = "right"; - } - else if (exchange_direction_ == "right") - { prefix_ = "LV5_L_"; - exchange_direction_ = "left"; - } + else + prefix_ = "LV5_R_"; if (exchange_arm_position_ == "normal") root_ = "EXCHANGE"; else root_ = "CAR_FRONT_EXCHANGE"; - ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftGPress() @@ -727,13 +717,13 @@ void Engineer2Manual::shiftGPress() else { exchange_arm_position_ = "normal"; - if (stone_state_[0]) + if (engineer_ui_.stone_num[0]) root_ = "G"; - else if (stone_state_[3]) + else if (engineer_ui_.stone_num[3]) root_ = "S3"; - else if (stone_state_[2]) + else if (engineer_ui_.stone_num[2]) root_ = "S2"; - else if (stone_state_[1]) + else if (engineer_ui_.stone_num[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -756,13 +746,13 @@ void Engineer2Manual::shiftQPress() else { exchange_arm_position_ = "normal"; - if (stone_state_[0]) + if (engineer_ui_.stone_num[0]) root_ = "G"; - else if (stone_state_[3]) + else if (engineer_ui_.stone_num[3]) root_ = "S3"; - else if (stone_state_[2]) + else if (engineer_ui_.stone_num[2]) root_ = "S2"; - else if (stone_state_[1]) + else if (engineer_ui_.stone_num[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -801,6 +791,22 @@ void Engineer2Manual::shiftXPress() } void Engineer2Manual::shiftZPress() { + if (exchange_direction_ == "left") + { + prefix_ = "LV5_R_"; + exchange_direction_ = "right"; + } + else if (exchange_direction_ == "right") + { + prefix_ = "LV5_L_"; + exchange_direction_ = "left"; + } + if (exchange_arm_position_ == "normal") + root_ = "EXCHANGE"; + else + root_ = "CAR_FRONT_EXCHANGE"; + ROS_INFO_STREAM(prefix_ + root_); + runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZRelease() { From 2be892dfb08d9b026d63e095dfa529fa7621f658 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 04:36:31 +0800 Subject: [PATCH 32/42] Seperate stone state and engineer ui. --- include/rm_manual/engineer2_manual.h | 7 ++-- src/engineer2_manual.cpp | 48 ++++++++++++++-------------- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index c68065df..061308bb 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include "unordered_map" @@ -144,7 +144,8 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ false }; + bool mouse_left_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_{}; @@ -158,7 +159,7 @@ class Engineer2Manual : public ChassisGimbalManual ros::Publisher engineer_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::Engineer2Ui engineer_ui_, old_ui_; + rm_msgs::EngineerUi engineer_ui_, old_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 156ad6aa..66e10302 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -10,7 +10,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere , operating_mode_(MANUAL) , action_client_("/engineer_middleware/move_steps", true) { - engineer_ui_pub_ = nh.advertise("/engineer2_ui", 10); + engineer_ui_pub_ = nh.advertise("/engineer_ui", 10); ROS_INFO("Waiting for middleware to start."); action_client_.waitForServer(); ROS_INFO("Middleware started."); @@ -223,7 +223,7 @@ void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { auto it = stoneNumMap_.find(data->data); if (it != stoneNumMap_.end()) - engineer_ui_.stone_num[it->second] = (data->data[0] == '+'); + stone_state_[it->second] = (data->data[0] == '+'); } void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) @@ -377,7 +377,7 @@ void Engineer2Manual::leftSwitchUpRise() had_side_gold_ = false; had_ground_stone_ = false; calibration_gather_->reset(); - for (auto& stone : engineer_ui_.stone_num) + for (auto& stone : stone_state_) { stone = false; } @@ -477,14 +477,14 @@ void Engineer2Manual::rPress() had_side_gold_ = false; if (had_ground_stone_) had_ground_stone_ = false; - else if (engineer_ui_.stone_num[0]) - engineer_ui_.stone_num[0] = false; - else if (engineer_ui_.stone_num[3]) - engineer_ui_.stone_num[3] = false; - else if (engineer_ui_.stone_num[2]) - engineer_ui_.stone_num[2] = false; - else if (engineer_ui_.stone_num[1]) - engineer_ui_.stone_num[1] = 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() @@ -591,7 +591,7 @@ void Engineer2Manual::ctrlRPress() calibration_gather_->reset(); ROS_INFO_STREAM("START CALIBRATE"); changeSpeedMode(NORMAL); - for (auto& stone : engineer_ui_.stone_num) + for (auto& stone : stone_state_) stone = false; runStepQueue("CA"); } @@ -678,13 +678,13 @@ void Engineer2Manual::shiftEPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -717,13 +717,13 @@ void Engineer2Manual::shiftGPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); @@ -746,13 +746,13 @@ void Engineer2Manual::shiftQPress() else { exchange_arm_position_ = "normal"; - if (engineer_ui_.stone_num[0]) + if (stone_state_[0]) root_ = "G"; - else if (engineer_ui_.stone_num[3]) + else if (stone_state_[3]) root_ = "S3"; - else if (engineer_ui_.stone_num[2]) + else if (stone_state_[2]) root_ = "S2"; - else if (engineer_ui_.stone_num[1]) + else if (stone_state_[1]) root_ = "S1"; } runStepQueue(prefix_ + root_); From 6341efb771d13599c7d4ec1d495f51a19f92247d Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 05:38:30 +0800 Subject: [PATCH 33/42] Modify function keys. --- src/engineer2_manual.cpp | 57 ++++++++++++++++++++-------------------- 1 file changed, 28 insertions(+), 29 deletions(-) diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 66e10302..55e99847 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -563,11 +563,15 @@ void Engineer2Manual::ctrlEPress() } void Engineer2Manual::ctrlFPress() { - prefix_ = "LV4_"; - root_ = "EXCHANGE"; - changeSpeedMode(EXCHANGE); + 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_); - ROS_INFO("%s", (prefix_ + root_).c_str()); } void Engineer2Manual::ctrlGPress() { @@ -625,6 +629,12 @@ void Engineer2Manual::ctrlWPress() } void Engineer2Manual::ctrlXPress() { + had_ground_stone_ = true; + prefix_ = ""; + root_ = "GROUND_STONE"; + changeSpeedMode(LOW); + ROS_INFO_STREAM(prefix_ + root_); + runStepQueue(prefix_ + root_); } void Engineer2Manual::ctrlZPress() { @@ -673,7 +683,7 @@ void Engineer2Manual::shiftEPress() else if (had_side_gold_) { exchange_arm_position_ = "front"; - root_ = "CAR_FRONT_EXCHANGE"; + root_ = "DROP_GOLD_EXCHANGE"; } else { @@ -692,13 +702,20 @@ void Engineer2Manual::shiftEPress() void Engineer2Manual::shiftFPress() { if (exchange_direction_ == "left") - prefix_ = "LV5_L_"; - else + { prefix_ = "LV5_R_"; + exchange_direction_ = "right"; + } + else if (exchange_direction_ == "right") + { + prefix_ = "LV5_L_"; + exchange_direction_ = "left"; + } if (exchange_arm_position_ == "normal") root_ = "EXCHANGE"; else root_ = "CAR_FRONT_EXCHANGE"; + ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftGPress() @@ -741,7 +758,7 @@ void Engineer2Manual::shiftQPress() else if (had_side_gold_) { exchange_arm_position_ = "front"; - root_ = "CAR_FRONT_EXCHANGE"; + root_ = "DROP_GOLD_EXCHANGE"; } else { @@ -782,31 +799,13 @@ void Engineer2Manual::shiftVRelease() } void Engineer2Manual::shiftXPress() { - had_ground_stone_ = true; - prefix_ = ""; - root_ = "GROUND_STONE"; - changeSpeedMode(LOW); - ROS_INFO_STREAM(prefix_ + root_); - runStepQueue(prefix_ + root_); } void Engineer2Manual::shiftZPress() { - if (exchange_direction_ == "left") - { - prefix_ = "LV5_R_"; - exchange_direction_ = "right"; - } - else if (exchange_direction_ == "right") - { - prefix_ = "LV5_L_"; - exchange_direction_ = "left"; - } - if (exchange_arm_position_ == "normal") - root_ = "EXCHANGE"; - else - root_ = "CAR_FRONT_EXCHANGE"; - ROS_INFO_STREAM(prefix_ + root_); + prefix_ = ""; + root_ = "CG"; runStepQueue(prefix_ + root_); + ROS_INFO("%s", (prefix_ + root_).c_str()); } void Engineer2Manual::shiftZRelease() { From c585ab8fc9b59483bf766da1caa3afd743e40080 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 08:58:24 +0800 Subject: [PATCH 34/42] Fix logic error. --- src/engineer2_manual.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 55e99847..6d7c7198 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -101,12 +101,11 @@ void Engineer2Manual::run() { ChassisGimbalManual::run(); calibration_gather_->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_); + if (engineer_ui_ != old_ui_) + { + engineer_ui_pub_.publish(engineer_ui_); + old_ui_ = engineer_ui_; + } } void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) @@ -286,14 +285,14 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& ROS_INFO("Done %s", (prefix_ + root_).c_str()); mouse_left_pressed_ = true; ROS_INFO("%i", result->finish); - engineer_ui_.symbol = UiState::NONE; - 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; @@ -393,11 +392,11 @@ void Engineer2Manual::leftSwitchDownRise() { if (!main_gripper_on_) { - runStepQueue("OPEN_MAIN_GRIPPER"); + runStepQueue("OM"); } else { - runStepQueue("CLOSE_MAIN_GRIPPER"); + runStepQueue("CM"); } } void Engineer2Manual::leftSwitchDownFall() @@ -555,7 +554,7 @@ void Engineer2Manual::ctrlDPress() engineer_ui_.symbol = UiState::SMALL_ISLAND; prefix_ = "GRIPPER_"; root_ = "THREE_SILVER"; - changeSpeedMode(LOW); + changeSpeedMode(EXCHANGE); runStepQueue(prefix_ + root_); } void Engineer2Manual::ctrlEPress() @@ -576,8 +575,8 @@ void Engineer2Manual::ctrlFPress() void Engineer2Manual::ctrlGPress() { engineer_ui_.symbol = UiState::BIG_ISLAND; - prefix_ = ""; - root_ = "MID_BIG_ISLAND"; + prefix_ = "MID_"; + root_ = "BIG_ISLAND"; changeSpeedMode(LOW); runStepQueue(prefix_ + root_); } From b19f8d9502cc113aa2fae2a04bdb9091d4766e25 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Thu, 1 Aug 2024 09:13:41 +0800 Subject: [PATCH 35/42] Add gripper ui publisher. --- include/rm_manual/engineer2_manual.h | 4 +++- src/engineer2_manual.cpp | 21 ++++----------------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 061308bb..60af39e1 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "unordered_map" @@ -156,10 +157,11 @@ class Engineer2Manual : public ChassisGimbalManual ros::Time last_time_; ros::Subscriber stone_num_sub_, gripper_state_sub_; - ros::Publisher engineer_ui_pub_; + 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_; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 6d7c7198..e4886f96 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -17,7 +17,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere 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); @@ -101,11 +101,8 @@ void Engineer2Manual::run() { ChassisGimbalManual::run(); calibration_gather_->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_); + gripper_ui_pub_.publish(gripper_ui_); } void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) @@ -227,17 +224,7 @@ void Engineer2Manual::stoneNumCallback(const std_msgs::String::ConstPtr& data) void Engineer2Manual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { - gpio_state_.gpio_state = data->gpio_state; - for (int i = 0; i <= 4; i++) - { - engineer_ui_.gripper_state[i] = data->gpio_state[i]; - } - engineer_ui_.gripper_state[5] = data->gpio_state[7]; - for (int i = 4; i >= 0; --i) - { - engineer_ui_.gripper_state[i] = data->gpio_state[i]; - } - engineer_ui_.gripper_state[5] = data->gpio_state[7]; + gripper_ui_.state = { data->gpio_state.begin(), data->gpio_state.end() - 2 }; main_gripper_on_ = data->gpio_state[0]; } From a36cd70065ec3c933949801afb279f07b873b1fd Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 2 Aug 2024 22:28:31 +0800 Subject: [PATCH 36/42] Modify yaw control direction in servo mode to sync with old engineer. --- src/engineer2_manual.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index e4886f96..ba4c2296 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -204,7 +204,7 @@ void Engineer2Manual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) 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); + 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) From ed273eff36a4144be1f9af8144a027d5eeaa7cbe Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 5 Aug 2024 23:24:21 +0800 Subject: [PATCH 37/42] Make gimbal switch between difference direction in different control mode. --- include/rm_manual/engineer2_manual.h | 2 +- src/engineer2_manual.cpp | 47 +++++++++++++++++++--------- 2 files changed, 33 insertions(+), 16 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index 60af39e1..d26f4407 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -151,7 +151,7 @@ class Engineer2Manual : public ChassisGimbalManual exchange_gyro_scale_{}, fast_speed_scale_{}, low_speed_scale_{}, normal_speed_scale_{}, exchange_speed_scale_{}; std::string prefix_{}, root_{}, exchange_direction_{ "left" }, exchange_arm_position_{ "normal" }; - int operating_mode_{}, servo_mode_{}, gimbal_mode_{}, gimbal_direction_{ 0 }; + int operating_mode_{}, servo_mode_{ 1 }, gimbal_mode_{}, gimbal_direction_{ 0 }; std::stack stone_num_{}; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index ba4c2296..8bb596da 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -379,11 +379,11 @@ void Engineer2Manual::leftSwitchDownRise() { if (!main_gripper_on_) { - runStepQueue("OM"); + runStepQueue("OA"); } else { - runStepQueue("CM"); + runStepQueue("CA"); } } void Engineer2Manual::leftSwitchDownFall() @@ -485,20 +485,37 @@ void Engineer2Manual::vRelease() } void Engineer2Manual::xPress() { - switch (gimbal_direction_) + if (servo_mode_ == SERVO) { - case 0: - runStepQueue("GIMBAL_R"); - gimbal_direction_ = 1; - break; - case 1: - runStepQueue("GIMBAL_L"); - gimbal_direction_ = 2; - break; - case 2: - runStepQueue("GIMBAL_F"); - gimbal_direction_ = 0; - break; + 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() From 3b6484dd2fcf36062e9134a4765eddb43e50ab90 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Mon, 5 Aug 2024 23:25:58 +0800 Subject: [PATCH 38/42] Remove useless function. --- include/rm_manual/engineer_manual.h | 17 +++++++---------- src/engineer_manual.cpp | 25 +++++++++++-------------- 2 files changed, 18 insertions(+), 24 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 4b90f828..1c2dff81 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -51,9 +51,9 @@ class EngineerManual : public ChassisGimbalManual enum ServoOrientation { - MID, - RIGHT, - LEFT + MID, + RIGHT, + LEFT }; EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); @@ -92,8 +92,6 @@ class EngineerManual : public ChassisGimbalManual void leftSwitchDownFall(); void ctrlAPress(); void ctrlBPress(); - void ctrlBPressing(); - void ctrlBRelease(); void ctrlCPress(); void ctrlDPress(); void ctrlEPress(); @@ -149,16 +147,16 @@ class EngineerManual : public ChassisGimbalManual // Servo - bool change_flag_{}, ore_rotator_pos_{ false }, - shift_z_pressed_{ false }, ore_lifter_on_{ false }, v_pressed_{ false }; + 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" }, last_ore_{}; - int operating_mode_{}, servo_mode_{}, servo_orientation_{ 0 }, gimbal_mode_{}, gimbal_height_{ 0 }, gimbal_direction_{ 0 }, - ore_lifter_pos_{ 0 }; + int operating_mode_{}, servo_mode_{}, servo_orientation_{ 0 }, gimbal_mode_{}, gimbal_height_{ 0 }, + gimbal_direction_{ 0 }, ore_lifter_pos_{ 0 }; std::stack stone_num_{}; @@ -174,7 +172,6 @@ class EngineerManual : public ChassisGimbalManual rm_common::CalibrationQueue *calibration_gather_{}, *pitch_calibration_, *ore_bin_lifter_calibration_{}; - actionlib::SimpleActionClient action_client_; InputEvent left_switch_up_event_, left_switch_down_event_, ctrl_a_event_, ctrl_b_event_, ctrl_c_event_, ctrl_d_event_, diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index 30480d36..c8cc9ead 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -49,8 +49,6 @@ EngineerManual::EngineerManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) ctrl_a_event_.setRising(boost::bind(&EngineerManual::ctrlAPress, this)); ctrl_b_event_.setRising(boost::bind(&EngineerManual::ctrlBPress, this)); ctrl_c_event_.setRising(boost::bind(&EngineerManual::ctrlCPress, this)); - ctrl_b_event_.setActiveHigh(boost::bind(&EngineerManual::ctrlBPressing, this)); - ctrl_b_event_.setFalling(boost::bind(&EngineerManual::ctrlBRelease, this)); ctrl_d_event_.setRising(boost::bind(&EngineerManual::ctrlDPress, this)); ctrl_e_event_.setRising(boost::bind(&EngineerManual::ctrlEPress, this)); ctrl_f_event_.setRising(boost::bind(&EngineerManual::ctrlFPress, this)); @@ -218,7 +216,6 @@ void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) 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) { @@ -465,12 +462,12 @@ void EngineerManual::ctrlAPress() void EngineerManual::ctrlBPress() { - prefix_ = ""; - root_ = "HOME"; - engineer_ui_.gripper_state = "CLOSED"; - runStepQueue(prefix_ + root_); - ROS_INFO_STREAM("RUN_HOME"); - changeSpeedMode(NORMAL); + prefix_ = ""; + root_ = "HOME"; + engineer_ui_.gripper_state = "CLOSED"; + runStepQueue(prefix_ + root_); + ROS_INFO_STREAM("RUN_HOME"); + changeSpeedMode(NORMAL); } void EngineerManual::ctrlCPress() @@ -494,7 +491,7 @@ void EngineerManual::ctrlEPress() void EngineerManual::ctrlFPress() { - if(root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") + if (root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") { prefix_ = ""; root_ = "RIGHT_EXCHANGE"; @@ -502,7 +499,7 @@ void EngineerManual::ctrlFPress() ROS_INFO("%s", (prefix_ + root_).c_str()); changeSpeedMode(EXCHANGE); } - else if(root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") + else if (root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") { prefix_ = ""; root_ = "LEFT_EXCHANGE"; @@ -804,7 +801,7 @@ void EngineerManual::shiftEPress() void EngineerManual::shiftFPress() { - if(root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") + if (root_ == "GET_DOWN_STONE_RIGHT" || root_ == "GET_UP_STONE_RIGHT" || root_ == "RIGHT_EXCHANGE") { prefix_ = ""; root_ = "EXCHANGE_POS"; @@ -813,7 +810,7 @@ void EngineerManual::shiftFPress() ROS_INFO("%s", (prefix_ + root_).c_str()); changeSpeedMode(EXCHANGE); } - else if(root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") + else if (root_ == "GET_DOWN_STONE_LEFT" || root_ == "GET_UP_STONE_LEFT" || root_ == "LEFT_EXCHANGE") { prefix_ = ""; root_ = "RIGHT_EXCHANGE"; @@ -822,7 +819,7 @@ void EngineerManual::shiftFPress() ROS_INFO("%s", (prefix_ + root_).c_str()); changeSpeedMode(EXCHANGE); } - else if(root_ == "GET_DOWN_STONE_BIN" || root_ == "GET_UP_STONE_BIN" || root_ == "EXCHANGE_POS") + else if (root_ == "GET_DOWN_STONE_BIN" || root_ == "GET_UP_STONE_BIN" || root_ == "EXCHANGE_POS") { prefix_ = ""; root_ = "LEFT_EXCHANGE"; From f6a79aa3adebc37bd7691c0cb568d66d73e01ed1 Mon Sep 17 00:00:00 2001 From: cc0h <7921166012@qq.com> Date: Fri, 9 Aug 2024 10:25:37 +0800 Subject: [PATCH 39/42] Add functions. --- include/rm_manual/engineer2_manual.h | 10 ++++---- src/engineer2_manual.cpp | 35 ++++++++++++++++++++++++---- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/include/rm_manual/engineer2_manual.h b/include/rm_manual/engineer2_manual.h index d26f4407..c7e4790e 100644 --- a/include/rm_manual/engineer2_manual.h +++ b/include/rm_manual/engineer2_manual.h @@ -48,7 +48,8 @@ class Engineer2Manual : public ChassisGimbalManual LOW, NORMAL, FAST, - EXCHANGE + EXCHANGE, + BIG_ISLAND_SPEED }; Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); @@ -145,10 +146,11 @@ class Engineer2Manual : public ChassisGimbalManual // Servo - bool mouse_left_pressed_{}, had_ground_stone_{ false }, main_gripper_on_{ false }, had_side_gold_{ false }, - stone_state_[4]{}; + 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_{}; + 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 }; diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 8bb596da..357eebe2 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -28,6 +28,7 @@ Engineer2Manual::Engineer2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_refere 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); @@ -125,6 +126,9 @@ void Engineer2Manual::changeSpeedMode(SpeedMode speed_mode) 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_; @@ -271,6 +275,7 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& 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") @@ -287,6 +292,26 @@ void Engineer2Manual::actionDoneCallback(const actionlib::SimpleClientGoalState& 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() @@ -551,6 +576,8 @@ void Engineer2Manual::ctrlBRelease() void Engineer2Manual::ctrlCPress() { action_client_.cancelAllGoals(); + changeSpeedMode(NORMAL); + initMode(); ROS_INFO("cancel all goal"); } void Engineer2Manual::ctrlDPress() @@ -706,18 +733,18 @@ void Engineer2Manual::shiftFPress() { if (exchange_direction_ == "left") { - prefix_ = "LV5_R_"; + prefix_ = "L2R_"; exchange_direction_ = "right"; } else if (exchange_direction_ == "right") { - prefix_ = "LV5_L_"; + prefix_ = "R2L_"; exchange_direction_ = "left"; } if (exchange_arm_position_ == "normal") root_ = "EXCHANGE"; else - root_ = "CAR_FRONT_EXCHANGE"; + root_ = "EXCHANGE"; ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } @@ -732,7 +759,7 @@ void Engineer2Manual::shiftGPress() else if (had_side_gold_) { exchange_arm_position_ = "front"; - root_ = "CAR_FRONT_EXCHANGE"; + root_ = "EXCHANGE"; } else { From 0a788b86393b5dcb5a091e9bc4e35fa0d13471e8 Mon Sep 17 00:00:00 2001 From: cc0h <792166012@qq.com> Date: Tue, 1 Oct 2024 00:27:10 +0800 Subject: [PATCH 40/42] Change the method to judge if rc is open. --- src/manual_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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_) { From 99d896602bb0bb19e03eb7c9484ef262886dd58d Mon Sep 17 00:00:00 2001 From: cc0h <792166012@qq.com> Date: Fri, 4 Oct 2024 14:37:54 +0800 Subject: [PATCH 41/42] Modify old_ui_ to previous_ui_ to get better readability. --- include/rm_manual/engineer_manual.h | 2 +- src/engineer_manual.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/rm_manual/engineer_manual.h b/include/rm_manual/engineer_manual.h index 1c2dff81..510fb787 100644 --- a/include/rm_manual/engineer_manual.h +++ b/include/rm_manual/engineer_manual.h @@ -165,7 +165,7 @@ class EngineerManual : public ChassisGimbalManual ros::Publisher engineer_ui_pub_; rm_msgs::GpioData gpio_state_; - rm_msgs::EngineerUi engineer_ui_, old_ui_; + rm_msgs::EngineerUi engineer_ui_, previous_ui_; rm_common::Vel3DCommandSender* servo_command_sender_; rm_common::ServiceCallerBase* servo_reset_caller_; diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index c8cc9ead..cd59e6cf 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -105,10 +105,10 @@ void EngineerManual::run() ChassisGimbalManual::run(); calibration_gather_->update(ros::Time::now()); ore_bin_lifter_calibration_->update(ros::Time::now()); - if (engineer_ui_ != old_ui_) + if (engineer_ui_ != previous_ui_) { engineer_ui_pub_.publish(engineer_ui_); - old_ui_ = engineer_ui_; + previous_ui_ = engineer_ui_; } } void EngineerManual::changeSpeedMode(SpeedMode speed_mode) From e0c3a104135f022b563d016cf7247653e16e7df7 Mon Sep 17 00:00:00 2001 From: cc0h <792166012@qq.com> Date: Fri, 4 Oct 2024 14:41:47 +0800 Subject: [PATCH 42/42] Improve format. --- src/engineer2_manual.cpp | 51 +++++++++++++++++++++++++++++++++++++++- src/engineer_manual.cpp | 34 +++++++++++++++++++++++++++ 2 files changed, 84 insertions(+), 1 deletion(-) diff --git a/src/engineer2_manual.cpp b/src/engineer2_manual.cpp index 357eebe2..36b62c69 100644 --- a/src/engineer2_manual.cpp +++ b/src/engineer2_manual.cpp @@ -361,6 +361,7 @@ void Engineer2Manual::rightSwitchUpRise() servo_mode_ = JOINT; chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } + void Engineer2Manual::rightSwitchMidRise() { ChassisGimbalManual::rightSwitchMidRise(); @@ -369,6 +370,7 @@ void Engineer2Manual::rightSwitchMidRise() gimbal_cmd_sender_->setZero(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } + void Engineer2Manual::rightSwitchDownRise() { ChassisGimbalManual::rightSwitchDownRise(); @@ -395,6 +397,7 @@ void Engineer2Manual::leftSwitchUpRise() engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } + void Engineer2Manual::leftSwitchUpFall() { runStepQueue("HOME_WITH_PITCH"); @@ -411,6 +414,7 @@ void Engineer2Manual::leftSwitchDownRise() runStepQueue("CA"); } } + void Engineer2Manual::leftSwitchDownFall() { runStepQueue("MIDDLE_PITCH_UP"); @@ -442,46 +446,57 @@ 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_) @@ -505,9 +520,11 @@ void Engineer2Manual::rRelease() void Engineer2Manual::vPressing() { } + void Engineer2Manual::vRelease() { } + void Engineer2Manual::xPress() { if (servo_mode_ == SERVO) @@ -543,10 +560,12 @@ void Engineer2Manual::xPress() } } } + void Engineer2Manual::zPressing() { angular_z_scale_ = 0.8; } + void Engineer2Manual::zRelease() { angular_z_scale_ = 0.; @@ -560,6 +579,7 @@ void Engineer2Manual::ctrlAPress() runStepQueue(prefix_ + root_); changeSpeedMode(LOW); } + void Engineer2Manual::ctrlBPress() { prefix_ = ""; @@ -567,12 +587,15 @@ void Engineer2Manual::ctrlBPress() runStepQueue(prefix_ + root_); changeSpeedMode(NORMAL); } + void Engineer2Manual::ctrlBPressing() { } + void Engineer2Manual::ctrlBRelease() { } + void Engineer2Manual::ctrlCPress() { action_client_.cancelAllGoals(); @@ -580,6 +603,7 @@ void Engineer2Manual::ctrlCPress() initMode(); ROS_INFO("cancel all goal"); } + void Engineer2Manual::ctrlDPress() { engineer_ui_.symbol = UiState::SMALL_ISLAND; @@ -588,9 +612,11 @@ void Engineer2Manual::ctrlDPress() changeSpeedMode(EXCHANGE); runStepQueue(prefix_ + root_); } + void Engineer2Manual::ctrlEPress() { } + void Engineer2Manual::ctrlFPress() { if (exchange_direction_ == "left") @@ -603,6 +629,7 @@ void Engineer2Manual::ctrlFPress() root_ = "DROP_GOLD_EXCHANGE"; runStepQueue(prefix_ + root_); } + void Engineer2Manual::ctrlGPress() { engineer_ui_.symbol = UiState::BIG_ISLAND; @@ -611,9 +638,11 @@ void Engineer2Manual::ctrlGPress() changeSpeedMode(LOW); runStepQueue(prefix_ + root_); } + void Engineer2Manual::ctrlQPress() { } + void Engineer2Manual::ctrlRPress() { runStepQueue("CALIBRATION"); @@ -629,6 +658,7 @@ void Engineer2Manual::ctrlRPress() stone = false; runStepQueue("CA"); } + void Engineer2Manual::ctrlSPress() { engineer_ui_.symbol = UiState::BIG_ISLAND; @@ -640,12 +670,15 @@ void Engineer2Manual::ctrlSPress() 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; @@ -657,6 +690,7 @@ void Engineer2Manual::ctrlWPress() runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } + void Engineer2Manual::ctrlXPress() { had_ground_stone_ = true; @@ -666,26 +700,30 @@ void Engineer2Manual::ctrlXPress() 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(); @@ -701,6 +739,7 @@ void Engineer2Manual::shiftCPress() } ROS_INFO("cancel all goal"); } + void Engineer2Manual::shiftEPress() { exchange_direction_ = "right"; @@ -729,6 +768,7 @@ void Engineer2Manual::shiftEPress() } runStepQueue(prefix_ + root_); } + void Engineer2Manual::shiftFPress() { if (exchange_direction_ == "left") @@ -748,6 +788,7 @@ void Engineer2Manual::shiftFPress() ROS_INFO_STREAM(prefix_ + root_); runStepQueue(prefix_ + root_); } + void Engineer2Manual::shiftGPress() { prefix_ = "LV4_"; @@ -776,6 +817,7 @@ void Engineer2Manual::shiftGPress() runStepQueue(prefix_ + root_); ROS_INFO_STREAM(prefix_ + root_); } + void Engineer2Manual::shiftQPress() { exchange_direction_ = "left"; @@ -805,12 +847,15 @@ void Engineer2Manual::shiftQPress() runStepQueue(prefix_ + root_); ROS_INFO_STREAM(prefix_ + root_); } + void Engineer2Manual::shiftRPress() { } + void Engineer2Manual::shiftRRelease() { } + void Engineer2Manual::shiftVPress() { prefix_ = ""; @@ -824,12 +869,15 @@ void Engineer2Manual::shiftVPress() } runStepQueue(root_); } + void Engineer2Manual::shiftVRelease() { } + void Engineer2Manual::shiftXPress() { } + void Engineer2Manual::shiftZPress() { prefix_ = ""; @@ -837,6 +885,7 @@ void Engineer2Manual::shiftZPress() runStepQueue(prefix_ + root_); ROS_INFO("%s", (prefix_ + root_).c_str()); } + void Engineer2Manual::shiftZRelease() { } diff --git a/src/engineer_manual.cpp b/src/engineer_manual.cpp index cd59e6cf..f0af42a7 100644 --- a/src/engineer_manual.cpp +++ b/src/engineer_manual.cpp @@ -111,6 +111,7 @@ void EngineerManual::run() previous_ui_ = engineer_ui_; } } + void EngineerManual::changeSpeedMode(SpeedMode speed_mode) { if (speed_mode == LOW) @@ -134,6 +135,7 @@ void EngineerManual::changeSpeedMode(SpeedMode speed_mode) gyro_scale_ = exchange_gyro_scale_; } } + void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::checkKeyboard(dbus_data); @@ -180,6 +182,7 @@ void EngineerManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) c_event_.update(dbus_data->key_c & !dbus_data->key_ctrl & !dbus_data->key_shift); } + void EngineerManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::updateRc(dbus_data); @@ -192,6 +195,7 @@ void EngineerManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) left_switch_up_event_.update(dbus_data->s_l == rm_msgs::DbusData::UP); left_switch_down_event_.update(dbus_data->s_l == rm_msgs::DbusData::DOWN); } + void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { checkKeyboard(dbus_data); @@ -200,6 +204,7 @@ void EngineerManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) if (servo_mode_ == JOINT) vel_cmd_sender_->setAngularZVel(-dbus_data->m_x * gimbal_scale_); } + void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) { switch (servo_orientation_) @@ -217,6 +222,7 @@ void EngineerManual::updateServo(const rm_msgs::DbusData::ConstPtr& dbus_data) 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) { ManualBase::dbusDataCallback(data); @@ -224,6 +230,7 @@ void EngineerManual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) if (servo_mode_ == SERVO) updateServo(data); } + void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) { if (data->data == "-1" && !stone_num_.empty()) @@ -240,6 +247,7 @@ void EngineerManual::stoneNumCallback(const std_msgs::String::ConstPtr& data) engineer_ui_.stone_num++; } } + void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) { gpio_state_.gpio_state = data->gpio_state; @@ -254,6 +262,7 @@ void EngineerManual::gpioStateCallback(const rm_msgs::GpioData::ConstPtr& data) engineer_ui_.gripper_state = "CLOSED"; } } + void EngineerManual::sendCommand(const ros::Time& time) { if (operating_mode_ == MANUAL) @@ -269,6 +278,7 @@ void EngineerManual::sendCommand(const ros::Time& time) gimbal_cmd_sender_->sendCommand(time); } } + void EngineerManual::runStepQueue(const std::string& step_queue_name) { rm_msgs::EngineerGoal goal; @@ -284,9 +294,11 @@ void EngineerManual::runStepQueue(const std::string& step_queue_name) else ROS_ERROR("Can not connect to middleware"); } + void EngineerManual::actionFeedbackCallback(const rm_msgs::EngineerFeedbackConstPtr& feedback) { } + void EngineerManual::actionDoneCallback(const actionlib::SimpleClientGoalState& state, const rm_msgs::EngineerResultConstPtr& result) { @@ -335,6 +347,7 @@ void EngineerManual::enterServo() chassis_cmd_sender_->getMsg()->command_source_frame = "link4"; engineer_ui_.control_mode = "SERVO"; } + void EngineerManual::initMode() { servo_mode_ = JOINT; @@ -357,6 +370,7 @@ void EngineerManual::gimbalOutputOn() pitch_calibration_->reset(); ROS_INFO("pitch calibrated"); } + void EngineerManual::chassisOutputOn() { if (operating_mode_ == MIDDLEWARE) @@ -371,6 +385,7 @@ void EngineerManual::rightSwitchUpRise() servo_mode_ = JOINT; chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } + void EngineerManual::rightSwitchMidRise() { ChassisGimbalManual::rightSwitchMidRise(); @@ -379,6 +394,7 @@ void EngineerManual::rightSwitchMidRise() gimbal_cmd_sender_->setZero(); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); } + void EngineerManual::rightSwitchDownRise() { ChassisGimbalManual::rightSwitchDownRise(); @@ -403,6 +419,7 @@ void EngineerManual::leftSwitchUpRise() engineer_ui_.control_mode = "NORMAL"; ROS_INFO_STREAM("START CALIBRATE"); } + void EngineerManual::leftSwitchUpFall() { runStepQueue("HOME"); @@ -422,9 +439,11 @@ void EngineerManual::leftSwitchDownRise() engineer_ui_.gripper_state = "CLOSED"; } } + void EngineerManual::leftSwitchDownFall() { } + // mouse input void EngineerManual::mouseLeftRelease() { @@ -436,6 +455,7 @@ void EngineerManual::mouseLeftRelease() ROS_INFO("Finished %s", (prefix_ + root_).c_str()); } } + //--------------------- keyboard input ------------------------ void EngineerManual::mouseRightRelease() { @@ -587,10 +607,12 @@ void EngineerManual::ctrlXPress() void EngineerManual::ctrlZPress() { } + //-------------------------- keys ------------------------------ void EngineerManual::bPressing() { } + void EngineerManual::bRelease() { } @@ -600,6 +622,7 @@ void EngineerManual::cPressing() angular_z_scale_ = 0.5; // ROS_INFO_STREAM("angular_z_scale is 0.5"); } + void EngineerManual::cRelease() { angular_z_scale_ = 0.; @@ -610,6 +633,7 @@ void EngineerManual::ePressing() if (servo_mode_ == SERVO) vel_cmd_sender_->setAngularZVel(-gyro_scale_); } + void EngineerManual::eRelease() { if (servo_mode_ == SERVO) @@ -634,6 +658,7 @@ void EngineerManual::fPress() break; } } + void EngineerManual::fRelease() { } @@ -647,6 +672,7 @@ void EngineerManual::gPress() } ROS_INFO_STREAM("Stone num is: " << stone_num_.size() << ", stone is " << stone_num_.top()); } + void EngineerManual::gRelease() { } @@ -656,6 +682,7 @@ void EngineerManual::qPressing() if (servo_mode_ == SERVO) vel_cmd_sender_->setAngularZVel(gyro_scale_); } + void EngineerManual::qRelease() { if (servo_mode_ == SERVO) @@ -689,6 +716,7 @@ void EngineerManual::vPressing() } } } + void EngineerManual::vRelease() { v_pressed_ = false; @@ -718,16 +746,19 @@ void EngineerManual::zPressing() angular_z_scale_ = -0.5; // ROS_INFO_STREAM("angular_z_scale is -0.5"); } + void EngineerManual::zRelease() { angular_z_scale_ = 0.; } + //----------------------------- shift ---------------------------- void EngineerManual::shiftPressing() { changeSpeedMode(FAST); ROS_DEBUG_STREAM("speed mode is fast"); } + void EngineerManual::shiftRelease() { changeSpeedMode(NORMAL); @@ -737,6 +768,7 @@ void EngineerManual::shiftRelease() void EngineerManual::shiftBPress() { } + void EngineerManual::shiftBRelease() { } @@ -917,6 +949,7 @@ void EngineerManual::shiftQPress() void EngineerManual::shiftRPress() { } + void EngineerManual::shiftRRelease() { } @@ -934,6 +967,7 @@ void EngineerManual::shiftVPress() engineer_ui_.gripper_state = "CLOSED"; } } + void EngineerManual::shiftVRelease() { }