diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 1858e33..2e3a71a 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,8 @@ -[0] +[1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA -[1] +[2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA -[2] +[3] command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA \ No newline at end of file diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 1858e33..2e3a71a 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,8 @@ -[0] +[1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA -[1] +[2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA -[2] +[3] command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA \ No newline at end of file diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 265ca45..3b8cbb6 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -25,7 +25,7 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; - constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 1858e33..2e3a71a 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,8 @@ -[0] +[1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA -[1] +[2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA -[2] +[3] command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA \ No newline at end of file diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 265ca45..3b8cbb6 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -25,7 +25,7 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; - constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 0490ee1..38e69c5 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -85,14 +85,14 @@ } config_file.close(); - std::cout << "SlamWrapper init success" << std::endl; + std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; } std::vector SlamWrapper::get_command(const int index) { const std::string section = std::to_string(index); const std::string full_key = section + ".command"; - auto it = arm_commands_.find(full_key); + const auto it = arm_commands_.find(full_key); if (it != arm_commands_.end()) { return it->second; } @@ -101,20 +101,20 @@ void SlamWrapper::control_robotic_arm() { const std::vector left_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA + 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA }; const std::vector right_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + 0XFE, 0XFE, 0X0F, 0X3C, 0X0A, 0X98, 0X07, 0X6F, 0X06, 0X91, 0X07, 0XCD, 0X0A, 0X93, 0X08, 0X56, 0X14, 0XFA }; const std::vector default_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x14, 0xFA }; std::cout << "control_robotic_arm start" << std::endl; serial_port_->write(left_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(5)); serial_port_->write(right_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(5)); serial_port_->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } @@ -183,13 +183,21 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + if (arrive_ != 2) { + std::cout << "I am not at node 2, please move to node 2." << std::endl; + return; + } + const std::vector command = get_command(arrive_); + if (command.empty()) { + std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + return; + } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_] { + std::thread countdown_thread([this,&arrive_, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - const std::vector command = get_command(arrive_); serial_port_->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 1858e33..2e3a71a 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,8 @@ -[0] +[1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA -[1] +[2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA -[2] +[3] command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA \ No newline at end of file diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 265ca45..3b8cbb6 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -25,7 +25,7 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; - constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 0490ee1..38e69c5 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -85,14 +85,14 @@ } config_file.close(); - std::cout << "SlamWrapper init success" << std::endl; + std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; } std::vector SlamWrapper::get_command(const int index) { const std::string section = std::to_string(index); const std::string full_key = section + ".command"; - auto it = arm_commands_.find(full_key); + const auto it = arm_commands_.find(full_key); if (it != arm_commands_.end()) { return it->second; } @@ -101,20 +101,20 @@ void SlamWrapper::control_robotic_arm() { const std::vector left_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA + 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA }; const std::vector right_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + 0XFE, 0XFE, 0X0F, 0X3C, 0X0A, 0X98, 0X07, 0X6F, 0X06, 0X91, 0X07, 0XCD, 0X0A, 0X93, 0X08, 0X56, 0X14, 0XFA }; const std::vector default_command = { - 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x14, 0xFA }; std::cout << "control_robotic_arm start" << std::endl; serial_port_->write(left_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(5)); serial_port_->write(right_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(5)); serial_port_->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } @@ -183,13 +183,21 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + if (arrive_ != 2) { + std::cout << "I am not at node 2, please move to node 2." << std::endl; + return; + } + const std::vector command = get_command(arrive_); + if (command.empty()) { + std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + return; + } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_] { + std::thread countdown_thread([this,&arrive_, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - const std::vector command = get_command(arrive_); serial_port_->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 8a3ad12..ef41ebe 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -59,7 +59,7 @@ void move_to_o(); - std::vector SlamWrapper::get_command(int index); + std::vector get_command(int index); //test end