diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index d5895cd..be4bf9d 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -127,6 +127,8 @@ << "\033[0m" << std::endl; } + load_command_config(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -135,7 +137,10 @@ // sub 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); +} +//casic start +void SlamWrapper::load_command_config() { // 读取本地指令配置 std::ifstream config_file(Config::ROBOTIC_ARM_COMMAND_CONFIG); if (!config_file.is_open()) { @@ -148,7 +153,9 @@ CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); - if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + if (line.empty()) continue; // 忽略空行 + + std::cout << line << std::endl; if (line[0] == '[') { // 处理节名 [section] @@ -156,7 +163,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -188,16 +195,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } config_file.close(); std::cout << "\033[1;92m" - << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " + << "SlamWrapper init success, has " << _command_map.size() << " arm commands " << "\033[0m" << std::endl; } -//casic start void SlamWrapper::control_robotic_arm() const { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -253,8 +259,8 @@ const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = _armCommands.find(full_key); - if (it != _armCommands.end() && it->second.enabled) { + const auto it = _command_map.find(full_key); + if (it != _command_map.end() && it->second.enabled) { return it->second.command; } return {}; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index d5895cd..be4bf9d 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -127,6 +127,8 @@ << "\033[0m" << std::endl; } + load_command_config(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -135,7 +137,10 @@ // sub 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); +} +//casic start +void SlamWrapper::load_command_config() { // 读取本地指令配置 std::ifstream config_file(Config::ROBOTIC_ARM_COMMAND_CONFIG); if (!config_file.is_open()) { @@ -148,7 +153,9 @@ CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); - if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + if (line.empty()) continue; // 忽略空行 + + std::cout << line << std::endl; if (line[0] == '[') { // 处理节名 [section] @@ -156,7 +163,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -188,16 +195,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } config_file.close(); std::cout << "\033[1;92m" - << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " + << "SlamWrapper init success, has " << _command_map.size() << " arm commands " << "\033[0m" << std::endl; } -//casic start void SlamWrapper::control_robotic_arm() const { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -253,8 +259,8 @@ const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = _armCommands.find(full_key); - if (it != _armCommands.end() && it->second.enabled) { + const auto it = _command_map.find(full_key); + if (it != _command_map.end() && it->second.enabled) { return it->second.command; } return {}; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8d4546..23b27ea 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,6 +58,8 @@ explicit SlamWrapper(); //casic start + void load_command_config(); + void control_robotic_arm() const; void move_to_i() const; @@ -126,29 +128,19 @@ 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 _armCommands; + std::map _command_map; int _current_node = 0; std::map _commandCaches; // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); + ChannelPublisherPtr pubQtCommand = std::make_shared >(COMMANDTOPIC); + ChannelPublisherPtr pubQtNode = std::make_shared >(ADDNODETOPIC); + ChannelPublisherPtr pubQtEdge = std::make_shared >(ADDEDGETOPIC); // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); + ChannelSubscriberPtr subQtNotice = std::make_shared >(NOTICETOPIC); + ChannelSubscriberPtr subOdometer = std::make_shared >(ODOMTOPIC); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index d5895cd..be4bf9d 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -127,6 +127,8 @@ << "\033[0m" << std::endl; } + load_command_config(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -135,7 +137,10 @@ // sub 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); +} +//casic start +void SlamWrapper::load_command_config() { // 读取本地指令配置 std::ifstream config_file(Config::ROBOTIC_ARM_COMMAND_CONFIG); if (!config_file.is_open()) { @@ -148,7 +153,9 @@ CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); - if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + if (line.empty()) continue; // 忽略空行 + + std::cout << line << std::endl; if (line[0] == '[') { // 处理节名 [section] @@ -156,7 +163,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -188,16 +195,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } config_file.close(); std::cout << "\033[1;92m" - << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " + << "SlamWrapper init success, has " << _command_map.size() << " arm commands " << "\033[0m" << std::endl; } -//casic start void SlamWrapper::control_robotic_arm() const { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -253,8 +259,8 @@ const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = _armCommands.find(full_key); - if (it != _armCommands.end() && it->second.enabled) { + const auto it = _command_map.find(full_key); + if (it != _command_map.end() && it->second.enabled) { return it->second.command; } return {}; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8d4546..23b27ea 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,6 +58,8 @@ explicit SlamWrapper(); //casic start + void load_command_config(); + void control_robotic_arm() const; void move_to_i() const; @@ -126,29 +128,19 @@ 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 _armCommands; + std::map _command_map; int _current_node = 0; std::map _commandCaches; // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); + ChannelPublisherPtr pubQtCommand = std::make_shared >(COMMANDTOPIC); + ChannelPublisherPtr pubQtNode = std::make_shared >(ADDNODETOPIC); + ChannelPublisherPtr pubQtEdge = std::make_shared >(ADDEDGETOPIC); // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); + ChannelSubscriberPtr subQtNotice = std::make_shared >(NOTICETOPIC); + ChannelSubscriberPtr subOdometer = std::make_shared >(ODOMTOPIC); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 37a4c39..8ca26c3 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -114,7 +114,7 @@ } } -void TcpService::update_node(const int node) { +void TcpService::update_node(const int node) const { std::stringstream ss; ss << node << "," << gas_value << "," << "116.296975" << "," << "39.990889"; const std::string message = ss.str(); @@ -124,7 +124,7 @@ } } -void TcpService::send_gas_warning(const int value) { +void TcpService::send_gas_warning(const int value) const { std::stringstream ss; ss << "2" << "," << value << "," << "116.296975" << "," << "39.990889"; const std::string message = ss.str(); diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index d5895cd..be4bf9d 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -127,6 +127,8 @@ << "\033[0m" << std::endl; } + load_command_config(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -135,7 +137,10 @@ // sub 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); +} +//casic start +void SlamWrapper::load_command_config() { // 读取本地指令配置 std::ifstream config_file(Config::ROBOTIC_ARM_COMMAND_CONFIG); if (!config_file.is_open()) { @@ -148,7 +153,9 @@ CommandAttribute current_command; while (std::getline(config_file, line)) { line = trim(line); - if (line.empty() || line[0] == '#') continue; // 忽略空行和注释 + if (line.empty()) continue; // 忽略空行 + + std::cout << line << std::endl; if (line[0] == '[') { // 处理节名 [section] @@ -156,7 +163,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -188,16 +195,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - _armCommands[current_section + ".command"] = current_command; + _command_map[current_section + ".command"] = current_command; } config_file.close(); std::cout << "\033[1;92m" - << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " + << "SlamWrapper init success, has " << _command_map.size() << " arm commands " << "\033[0m" << std::endl; } -//casic start void SlamWrapper::control_robotic_arm() const { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -253,8 +259,8 @@ const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = _armCommands.find(full_key); - if (it != _armCommands.end() && it->second.enabled) { + const auto it = _command_map.find(full_key); + if (it != _command_map.end() && it->second.enabled) { return it->second.command; } return {}; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8d4546..23b27ea 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,6 +58,8 @@ explicit SlamWrapper(); //casic start + void load_command_config(); + void control_robotic_arm() const; void move_to_i() const; @@ -126,29 +128,19 @@ 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 _armCommands; + std::map _command_map; int _current_node = 0; std::map _commandCaches; // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); + ChannelPublisherPtr pubQtCommand = std::make_shared >(COMMANDTOPIC); + ChannelPublisherPtr pubQtNode = std::make_shared >(ADDNODETOPIC); + ChannelPublisherPtr pubQtEdge = std::make_shared >(ADDEDGETOPIC); // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); + ChannelSubscriberPtr subQtNotice = std::make_shared >(NOTICETOPIC); + ChannelSubscriberPtr subOdometer = std::make_shared >(ODOMTOPIC); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 37a4c39..8ca26c3 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -114,7 +114,7 @@ } } -void TcpService::update_node(const int node) { +void TcpService::update_node(const int node) const { std::stringstream ss; ss << node << "," << gas_value << "," << "116.296975" << "," << "39.990889"; const std::string message = ss.str(); @@ -124,7 +124,7 @@ } } -void TcpService::send_gas_warning(const int value) { +void TcpService::send_gas_warning(const int value) const { std::stringstream ss; ss << "2" << "," << value << "," << "116.296975" << "," << "39.990889"; const std::string message = ss.str(); diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index 822612a..e96dcc9 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -27,7 +27,7 @@ * 更新节点信息 * @param node */ - void update_node(int node); + void update_node(int node) const; /** * 更新甲烷浓度数据 @@ -35,7 +35,7 @@ */ void update_gas_value(int value); - void send_gas_warning(int value); + void send_gas_warning(int value) const; private: TcpService() = default;