diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 2e3a71a..0fbc584 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,11 @@ [1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +enabled=0 [2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +enabled=1 [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 +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +enabled=0 \ No newline at end of file diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 2e3a71a..0fbc584 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,11 @@ [1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +enabled=0 [2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +enabled=1 [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 +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +enabled=0 \ No newline at end of file diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 38e69c5..8d9c373 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -52,6 +52,7 @@ std::string line; std::string current_section; + CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 @@ -60,7 +61,13 @@ // 处理节名 [section] size_t end_bracket = line.find(']'); if (end_bracket != std::string::npos) { + if (!current_section.empty()) { + // 将上一个节的数据存储到 map 中 + arm_commands_[current_section + ".command"] = current_command; + } current_section = line.substr(1, end_bracket - 1); + // 初始化新的节数据 + current_command = {}; } } else { size_t delimiter_pos = line.find('='); @@ -68,21 +75,28 @@ 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; + if (key == "command") { + // 将字符串形式的 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))); + while (ss >> byte_str) { + bytes.push_back(static_cast(std::strtoul(byte_str.c_str(), nullptr, 16))); + } + + current_command.command = bytes; + } else if (key == "enabled") { + // 解析启用状态 + current_command.enabled = value == "1"; } - - // 使用 section.key 作为 map 的 key - std::string full_key = current_section + "." + key; - arm_commands_[full_key] = bytes; } } } + // 存储最后一个节的数据 + if (!current_section.empty()) { + arm_commands_[current_section + ".command"] = current_command; + } config_file.close(); std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; @@ -93,8 +107,8 @@ const std::string full_key = section + ".command"; const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end()) { - return it->second; + if (it != arm_commands_.end() && it->second.enabled) { + return it->second.command; } return {}; } @@ -183,10 +197,6 @@ 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; diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 2e3a71a..0fbc584 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,8 +1,11 @@ [1] command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +enabled=0 [2] command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +enabled=1 [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 +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +enabled=0 \ No newline at end of file diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 38e69c5..8d9c373 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -52,6 +52,7 @@ std::string line; std::string current_section; + CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 @@ -60,7 +61,13 @@ // 处理节名 [section] size_t end_bracket = line.find(']'); if (end_bracket != std::string::npos) { + if (!current_section.empty()) { + // 将上一个节的数据存储到 map 中 + arm_commands_[current_section + ".command"] = current_command; + } current_section = line.substr(1, end_bracket - 1); + // 初始化新的节数据 + current_command = {}; } } else { size_t delimiter_pos = line.find('='); @@ -68,21 +75,28 @@ 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; + if (key == "command") { + // 将字符串形式的 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))); + while (ss >> byte_str) { + bytes.push_back(static_cast(std::strtoul(byte_str.c_str(), nullptr, 16))); + } + + current_command.command = bytes; + } else if (key == "enabled") { + // 解析启用状态 + current_command.enabled = value == "1"; } - - // 使用 section.key 作为 map 的 key - std::string full_key = current_section + "." + key; - arm_commands_[full_key] = bytes; } } } + // 存储最后一个节的数据 + if (!current_section.empty()) { + arm_commands_[current_section + ".command"] = current_command; + } config_file.close(); std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; @@ -93,8 +107,8 @@ const std::string full_key = section + ".command"; const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end()) { - return it->second; + if (it != arm_commands_.end() && it->second.enabled) { + return it->second.command; } return {}; } @@ -183,10 +197,6 @@ 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; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index ef41ebe..2c76b60 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,6 +44,11 @@ u_int16_t edgeEnd; }; +struct CommandAttribute { + std::vector command; + bool enabled; +}; + class SlamWrapper { public: std::vector node_attributes; @@ -102,7 +107,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_; + std::map arm_commands_; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr(