diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf new file mode 100644 index 0000000..1858e33 --- /dev/null +++ b/robotic_arm_commands.conf @@ -0,0 +1,8 @@ +[0] +command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA + +[1] +command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA + +[2] +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 new file mode 100644 index 0000000..1858e33 --- /dev/null +++ b/robotic_arm_commands.conf @@ -0,0 +1,8 @@ +[0] +command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA + +[1] +command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA + +[2] +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 ff5171e..265ca45 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -6,9 +6,11 @@ #define CONSTANT_CONFIG_HPP namespace ConstantConfig { - constexpr int TCP_REMOTE_PORT = 30029; + constexpr int TCP_REMOTE_PORT = 9001; + // constexpr int TCP_REMOTE_PORT = 30029; - constexpr const char *SERVICE_ADDRESS = "192.168.123.125"; + constexpr const char *SERVICE_ADDRESS = "192.168.123.18"; + // constexpr const char *SERVICE_ADDRESS = "111.198.10.15"; constexpr int TCP_LOCALE_PORT = 8888; @@ -22,6 +24,8 @@ constexpr const char *SLAM_NET_PORT = "eth0"; constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; + + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf new file mode 100644 index 0000000..1858e33 --- /dev/null +++ b/robotic_arm_commands.conf @@ -0,0 +1,8 @@ +[0] +command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA + +[1] +command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA + +[2] +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 ff5171e..265ca45 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -6,9 +6,11 @@ #define CONSTANT_CONFIG_HPP namespace ConstantConfig { - constexpr int TCP_REMOTE_PORT = 30029; + constexpr int TCP_REMOTE_PORT = 9001; + // constexpr int TCP_REMOTE_PORT = 30029; - constexpr const char *SERVICE_ADDRESS = "192.168.123.125"; + constexpr const char *SERVICE_ADDRESS = "192.168.123.18"; + // constexpr const char *SERVICE_ADDRESS = "111.198.10.15"; constexpr int TCP_LOCALE_PORT = 8888; @@ -22,6 +24,8 @@ constexpr const char *SLAM_NET_PORT = "eth0"; constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; + + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cb0bd6b..0490ee1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -8,6 +8,20 @@ #include #include +#include +#include + +static std::string trim(const std::string &s) { + auto start = s.begin(); + while (start != s.end() && std::isspace(*start)) ++start; + + auto end = s.end(); + do { + --end; + } while (std::distance(start, end) > 0 && std::isspace(*end)); + + return std::string(start, end + 1); +} SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, ConstantConfig::SLAM_NET_PORT); @@ -29,9 +43,62 @@ subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); + // 读取本地指令配置 + std::ifstream config_file(ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG); + if (!config_file.is_open()) { + std::cerr << "Failed to open config file" << std::endl; + return; + } + + std::string line; + std::string current_section; + while (std::getline(config_file, line)) { + line = trim(line); + if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + + if (line[0] == '[') { + // 处理节名 [section] + size_t end_bracket = line.find(']'); + if (end_bracket != std::string::npos) { + current_section = line.substr(1, end_bracket - 1); + } + } else { + size_t delimiter_pos = line.find('='); + if (delimiter_pos != std::string::npos) { + std::string key = line.substr(0, delimiter_pos); + std::string value = line.substr(delimiter_pos + 1); + + // 将字符串形式的 hex 指令转为 uint8_t vector + std::istringstream ss(value); + std::string byte_str; + std::vector bytes; + + while (ss >> byte_str) { + bytes.push_back(static_cast(std::strtoul(byte_str.c_str(), nullptr, 16))); + } + + // 使用 section.key 作为 map 的 key + std::string full_key = current_section + "." + key; + arm_commands_[full_key] = bytes; + } + } + } + config_file.close(); + std::cout << "SlamWrapper init success" << 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); + if (it != arm_commands_.end()) { + return it->second; + } + return {}; +} + 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 @@ -121,14 +188,9 @@ //倒计时结束后恢复巡检 std::thread countdown_thread([this,&arrive_] { try { - //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + const std::vector command = get_command(arrive_); + serial_port_->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf new file mode 100644 index 0000000..1858e33 --- /dev/null +++ b/robotic_arm_commands.conf @@ -0,0 +1,8 @@ +[0] +command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA + +[1] +command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA + +[2] +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 ff5171e..265ca45 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -6,9 +6,11 @@ #define CONSTANT_CONFIG_HPP namespace ConstantConfig { - constexpr int TCP_REMOTE_PORT = 30029; + constexpr int TCP_REMOTE_PORT = 9001; + // constexpr int TCP_REMOTE_PORT = 30029; - constexpr const char *SERVICE_ADDRESS = "192.168.123.125"; + constexpr const char *SERVICE_ADDRESS = "192.168.123.18"; + // constexpr const char *SERVICE_ADDRESS = "111.198.10.15"; constexpr int TCP_LOCALE_PORT = 8888; @@ -22,6 +24,8 @@ constexpr const char *SLAM_NET_PORT = "eth0"; constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; + + constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/user/robotic_arm_commands.conf"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cb0bd6b..0490ee1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -8,6 +8,20 @@ #include #include +#include +#include + +static std::string trim(const std::string &s) { + auto start = s.begin(); + while (start != s.end() && std::isspace(*start)) ++start; + + auto end = s.end(); + do { + --end; + } while (std::distance(start, end) > 0 && std::isspace(*end)); + + return std::string(start, end + 1); +} SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, ConstantConfig::SLAM_NET_PORT); @@ -29,9 +43,62 @@ subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); + // 读取本地指令配置 + std::ifstream config_file(ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG); + if (!config_file.is_open()) { + std::cerr << "Failed to open config file" << std::endl; + return; + } + + std::string line; + std::string current_section; + while (std::getline(config_file, line)) { + line = trim(line); + if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + + if (line[0] == '[') { + // 处理节名 [section] + size_t end_bracket = line.find(']'); + if (end_bracket != std::string::npos) { + current_section = line.substr(1, end_bracket - 1); + } + } else { + size_t delimiter_pos = line.find('='); + if (delimiter_pos != std::string::npos) { + std::string key = line.substr(0, delimiter_pos); + std::string value = line.substr(delimiter_pos + 1); + + // 将字符串形式的 hex 指令转为 uint8_t vector + std::istringstream ss(value); + std::string byte_str; + std::vector bytes; + + while (ss >> byte_str) { + bytes.push_back(static_cast(std::strtoul(byte_str.c_str(), nullptr, 16))); + } + + // 使用 section.key 作为 map 的 key + std::string full_key = current_section + "." + key; + arm_commands_[full_key] = bytes; + } + } + } + config_file.close(); + std::cout << "SlamWrapper init success" << 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); + if (it != arm_commands_.end()) { + return it->second; + } + return {}; +} + 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 @@ -121,14 +188,9 @@ //倒计时结束后恢复巡检 std::thread countdown_thread([this,&arrive_] { try { - //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + const std::vector command = get_command(arrive_); + serial_port_->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 4b962a6..8a3ad12 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -59,6 +59,8 @@ void move_to_o(); + std::vector SlamWrapper::get_command(int index); + //test end void qt_notice_handler(const void *message); @@ -100,6 +102,7 @@ const std::vector default_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA }; + std::map > arm_commands_; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr(