diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cecbe58..ac929f0 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -116,7 +116,7 @@ std::thread serial_port_thread([this] { //初始化机械臂串口 boost::asio::io_service io; - serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); + _serialPortPtr = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); io.run(); }); serial_port_thread.detach(); @@ -150,7 +150,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -182,15 +182,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } config_file.close(); - std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; + std::cout << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " << std::endl; } //casic start -void SlamWrapper::control_robotic_arm() { +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 }; @@ -199,15 +199,15 @@ }; std::cout << "control_robotic_arm start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } -void SlamWrapper::move_to_i() { +void SlamWrapper::move_to_i() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA }; @@ -216,15 +216,15 @@ }; std::cout << "move_to_i start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(6)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(9)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_i end" << std::endl; } -void SlamWrapper::move_to_o() { +void SlamWrapper::move_to_o() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA }; @@ -233,25 +233,98 @@ }; std::cout << "move_to_o start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_o end" << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); +std::vector SlamWrapper::get_command() const { + const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { + const auto it = _armCommands.find(full_key); + if (it != _armCommands.end() && it->second.enabled) { return it->second.command; } return {}; } +void SlamWrapper::arm_power_off() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); +} + +void SlamWrapper::arm_power_on() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // // 等待1秒 + // std::thread([this] { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // // 查询机械臂电位 + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + // const auto response = _serialPortPtr->read_from_port(); + // // 组帧，如：0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA + // std::vector cmd_frame = {}; + // cmd_frame = { + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, + // + // 0x0B, response[11], 0x0C, response[12], + // 0x0D, response[13], 0x0E, response[14], + // }; + // + // // 打印组的帧 + // for (size_t i = 0; i < cmd_frame.size(); ++i) { + // std::cout << std::hex + // << std::uppercase + // << std::setw(2) + // << std::setfill('0') + // << static_cast(cmd_frame[i]) + // << " "; + // } + // + // // 缓存起来，后面保存所有点的时候一起保存 + // CommandAttribute current_command; + // current_command.command = cmd_frame; + // current_command.enabled = true; + // _commandCaches[_current_node + ".command"] = current_command; + // }).detach(); +} + +void SlamWrapper::reset_arm_position() const { + _serialPortPtr->write(default_command); +} + +void SlamWrapper::send_gas_warning() const { + TcpService::getInstance().send_gas_warning(10000); +} + +void SlamWrapper::arm_turn_let_top() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_center() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x09, 0xd1, 0x07, 0xd9, 0x08, 0x11, 0x08, 0x36, 0x08, 0x2a, 0x08, 0x03, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_bottom() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0x1a, 0x07, 0x65, 0x08, 0x99, 0x07, 0xb8, 0x0a, 0x8d, 0x07, 0xdb, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_forward() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x98, 0x07, 0x6F, 0x06, 0x91, 0x07, 0xCD, 0x0A, 0x93, 0x08, 0x56, 0x14, 0xFA + }); +} + //casic end void SlamWrapper::qt_notice_handler(const void *message) { @@ -282,30 +355,30 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); 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; - const std::vector command = get_command(arrive_); + _current_node = atoi(str_.c_str()); + std::cout << "I arrived node " << _current_node << ". " << notice_ << std::endl; + const std::vector command = get_command(); if (command.empty()) { - std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + std::cout << "No command found for node " << _current_node << ". " << notice_ << std::endl; return; } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_, command] { + std::thread countdown_thread([this, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write(command); + _serialPortPtr->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 - TcpService::getInstance().update_node(arrive_); + TcpService::getInstance().update_node(_current_node); std::this_thread::sleep_for(std::chrono::seconds(5)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - serial_port_->write(default_command); + _serialPortPtr->write(default_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } @@ -410,6 +483,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } + // arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cecbe58..ac929f0 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -116,7 +116,7 @@ std::thread serial_port_thread([this] { //初始化机械臂串口 boost::asio::io_service io; - serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); + _serialPortPtr = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); io.run(); }); serial_port_thread.detach(); @@ -150,7 +150,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -182,15 +182,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } config_file.close(); - std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; + std::cout << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " << std::endl; } //casic start -void SlamWrapper::control_robotic_arm() { +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 }; @@ -199,15 +199,15 @@ }; std::cout << "control_robotic_arm start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } -void SlamWrapper::move_to_i() { +void SlamWrapper::move_to_i() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA }; @@ -216,15 +216,15 @@ }; std::cout << "move_to_i start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(6)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(9)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_i end" << std::endl; } -void SlamWrapper::move_to_o() { +void SlamWrapper::move_to_o() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA }; @@ -233,25 +233,98 @@ }; std::cout << "move_to_o start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_o end" << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); +std::vector SlamWrapper::get_command() const { + const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { + const auto it = _armCommands.find(full_key); + if (it != _armCommands.end() && it->second.enabled) { return it->second.command; } return {}; } +void SlamWrapper::arm_power_off() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); +} + +void SlamWrapper::arm_power_on() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // // 等待1秒 + // std::thread([this] { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // // 查询机械臂电位 + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + // const auto response = _serialPortPtr->read_from_port(); + // // 组帧，如：0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA + // std::vector cmd_frame = {}; + // cmd_frame = { + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, + // + // 0x0B, response[11], 0x0C, response[12], + // 0x0D, response[13], 0x0E, response[14], + // }; + // + // // 打印组的帧 + // for (size_t i = 0; i < cmd_frame.size(); ++i) { + // std::cout << std::hex + // << std::uppercase + // << std::setw(2) + // << std::setfill('0') + // << static_cast(cmd_frame[i]) + // << " "; + // } + // + // // 缓存起来，后面保存所有点的时候一起保存 + // CommandAttribute current_command; + // current_command.command = cmd_frame; + // current_command.enabled = true; + // _commandCaches[_current_node + ".command"] = current_command; + // }).detach(); +} + +void SlamWrapper::reset_arm_position() const { + _serialPortPtr->write(default_command); +} + +void SlamWrapper::send_gas_warning() const { + TcpService::getInstance().send_gas_warning(10000); +} + +void SlamWrapper::arm_turn_let_top() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_center() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x09, 0xd1, 0x07, 0xd9, 0x08, 0x11, 0x08, 0x36, 0x08, 0x2a, 0x08, 0x03, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_bottom() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0x1a, 0x07, 0x65, 0x08, 0x99, 0x07, 0xb8, 0x0a, 0x8d, 0x07, 0xdb, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_forward() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x98, 0x07, 0x6F, 0x06, 0x91, 0x07, 0xCD, 0x0A, 0x93, 0x08, 0x56, 0x14, 0xFA + }); +} + //casic end void SlamWrapper::qt_notice_handler(const void *message) { @@ -282,30 +355,30 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); 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; - const std::vector command = get_command(arrive_); + _current_node = atoi(str_.c_str()); + std::cout << "I arrived node " << _current_node << ". " << notice_ << std::endl; + const std::vector command = get_command(); if (command.empty()) { - std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + std::cout << "No command found for node " << _current_node << ". " << notice_ << std::endl; return; } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_, command] { + std::thread countdown_thread([this, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write(command); + _serialPortPtr->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 - TcpService::getInstance().update_node(arrive_); + TcpService::getInstance().update_node(_current_node); std::this_thread::sleep_for(std::chrono::seconds(5)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - serial_port_->write(default_command); + _serialPortPtr->write(default_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } @@ -410,6 +483,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } + // arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 0fec4c3..e438262 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,13 +58,30 @@ explicit SlamWrapper(); //casic start - void control_robotic_arm(); + void control_robotic_arm() const; - void move_to_i(); + void move_to_i() const; - void move_to_o(); + void move_to_o() const; - std::vector get_command(int index); + std::vector get_command() const; + + void arm_power_off() const; + + void arm_power_on() const; + + void reset_arm_position() const; + + void send_gas_warning() const; + + void arm_turn_let_top() const; + + void arm_turn_let_center() const; + + void arm_turn_let_bottom() const; + + void arm_turn_let_forward() const; + //casic end void qt_notice_handler(const void *message); @@ -102,11 +119,15 @@ private: int index = 0; const Odometry_ *currentOdom{}; - RoboticArmSerialPort *serial_port_; + + RoboticArmSerialPort *_serialPortPtr; 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 _armCommands; + + int _current_node = 0; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cecbe58..ac929f0 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -116,7 +116,7 @@ std::thread serial_port_thread([this] { //初始化机械臂串口 boost::asio::io_service io; - serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); + _serialPortPtr = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); io.run(); }); serial_port_thread.detach(); @@ -150,7 +150,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -182,15 +182,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } config_file.close(); - std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; + std::cout << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " << std::endl; } //casic start -void SlamWrapper::control_robotic_arm() { +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 }; @@ -199,15 +199,15 @@ }; std::cout << "control_robotic_arm start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } -void SlamWrapper::move_to_i() { +void SlamWrapper::move_to_i() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA }; @@ -216,15 +216,15 @@ }; std::cout << "move_to_i start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(6)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(9)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_i end" << std::endl; } -void SlamWrapper::move_to_o() { +void SlamWrapper::move_to_o() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA }; @@ -233,25 +233,98 @@ }; std::cout << "move_to_o start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_o end" << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); +std::vector SlamWrapper::get_command() const { + const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { + const auto it = _armCommands.find(full_key); + if (it != _armCommands.end() && it->second.enabled) { return it->second.command; } return {}; } +void SlamWrapper::arm_power_off() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); +} + +void SlamWrapper::arm_power_on() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // // 等待1秒 + // std::thread([this] { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // // 查询机械臂电位 + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + // const auto response = _serialPortPtr->read_from_port(); + // // 组帧，如：0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA + // std::vector cmd_frame = {}; + // cmd_frame = { + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, + // + // 0x0B, response[11], 0x0C, response[12], + // 0x0D, response[13], 0x0E, response[14], + // }; + // + // // 打印组的帧 + // for (size_t i = 0; i < cmd_frame.size(); ++i) { + // std::cout << std::hex + // << std::uppercase + // << std::setw(2) + // << std::setfill('0') + // << static_cast(cmd_frame[i]) + // << " "; + // } + // + // // 缓存起来，后面保存所有点的时候一起保存 + // CommandAttribute current_command; + // current_command.command = cmd_frame; + // current_command.enabled = true; + // _commandCaches[_current_node + ".command"] = current_command; + // }).detach(); +} + +void SlamWrapper::reset_arm_position() const { + _serialPortPtr->write(default_command); +} + +void SlamWrapper::send_gas_warning() const { + TcpService::getInstance().send_gas_warning(10000); +} + +void SlamWrapper::arm_turn_let_top() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_center() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x09, 0xd1, 0x07, 0xd9, 0x08, 0x11, 0x08, 0x36, 0x08, 0x2a, 0x08, 0x03, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_bottom() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0x1a, 0x07, 0x65, 0x08, 0x99, 0x07, 0xb8, 0x0a, 0x8d, 0x07, 0xdb, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_forward() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x98, 0x07, 0x6F, 0x06, 0x91, 0x07, 0xCD, 0x0A, 0x93, 0x08, 0x56, 0x14, 0xFA + }); +} + //casic end void SlamWrapper::qt_notice_handler(const void *message) { @@ -282,30 +355,30 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); 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; - const std::vector command = get_command(arrive_); + _current_node = atoi(str_.c_str()); + std::cout << "I arrived node " << _current_node << ". " << notice_ << std::endl; + const std::vector command = get_command(); if (command.empty()) { - std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + std::cout << "No command found for node " << _current_node << ". " << notice_ << std::endl; return; } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_, command] { + std::thread countdown_thread([this, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write(command); + _serialPortPtr->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 - TcpService::getInstance().update_node(arrive_); + TcpService::getInstance().update_node(_current_node); std::this_thread::sleep_for(std::chrono::seconds(5)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - serial_port_->write(default_command); + _serialPortPtr->write(default_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } @@ -410,6 +483,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } + // arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 0fec4c3..e438262 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,13 +58,30 @@ explicit SlamWrapper(); //casic start - void control_robotic_arm(); + void control_robotic_arm() const; - void move_to_i(); + void move_to_i() const; - void move_to_o(); + void move_to_o() const; - std::vector get_command(int index); + std::vector get_command() const; + + void arm_power_off() const; + + void arm_power_on() const; + + void reset_arm_position() const; + + void send_gas_warning() const; + + void arm_turn_let_top() const; + + void arm_turn_let_center() const; + + void arm_turn_let_bottom() const; + + void arm_turn_let_forward() const; + //casic end void qt_notice_handler(const void *message); @@ -102,11 +119,15 @@ private: int index = 0; const Odometry_ *currentOdom{}; - RoboticArmSerialPort *serial_port_; + + RoboticArmSerialPort *_serialPortPtr; 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 _armCommands; + + int _current_node = 0; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index e819793..22dc995 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -72,6 +72,30 @@ _slam.move_to_i(); } else if (received_string == "o") { _slam.move_to_o(); + } else if (received_string == "b") { + std::cout << "reset arm position" << std::endl; + _slam.reset_arm_position(); + } else if (received_string == "f") { + std::cout << "arm power off" << std::endl; + _slam.arm_power_off(); + } else if (received_string == "g") { + std::cout << "arm power on" << std::endl; + _slam.arm_power_on(); + } else if (received_string == "h") { + std::cout << "send gas warning" << std::endl; + _slam.send_gas_warning(); + } else if (received_string == "j") { + std::cout << "arm turn let top" << std::endl; + _slam.arm_turn_let_top(); + } else if (received_string == "k") { + std::cout << "arm turn let center" << std::endl; + _slam.arm_turn_let_center(); + } else if (received_string == "l") { + std::cout << "arm turn let bottom" << std::endl; + _slam.arm_turn_let_bottom(); + } else if (received_string == "m") { + std::cout << "arm turn let forward" << std::endl; + _slam.arm_turn_let_forward(); } else { std::cout << "Unknown command" << std::endl; } diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cecbe58..ac929f0 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -116,7 +116,7 @@ std::thread serial_port_thread([this] { //初始化机械臂串口 boost::asio::io_service io; - serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); + _serialPortPtr = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); io.run(); }); serial_port_thread.detach(); @@ -150,7 +150,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -182,15 +182,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } config_file.close(); - std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; + std::cout << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " << std::endl; } //casic start -void SlamWrapper::control_robotic_arm() { +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 }; @@ -199,15 +199,15 @@ }; std::cout << "control_robotic_arm start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } -void SlamWrapper::move_to_i() { +void SlamWrapper::move_to_i() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA }; @@ -216,15 +216,15 @@ }; std::cout << "move_to_i start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(6)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(9)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_i end" << std::endl; } -void SlamWrapper::move_to_o() { +void SlamWrapper::move_to_o() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA }; @@ -233,25 +233,98 @@ }; std::cout << "move_to_o start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_o end" << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); +std::vector SlamWrapper::get_command() const { + const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { + const auto it = _armCommands.find(full_key); + if (it != _armCommands.end() && it->second.enabled) { return it->second.command; } return {}; } +void SlamWrapper::arm_power_off() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); +} + +void SlamWrapper::arm_power_on() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // // 等待1秒 + // std::thread([this] { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // // 查询机械臂电位 + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + // const auto response = _serialPortPtr->read_from_port(); + // // 组帧，如：0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA + // std::vector cmd_frame = {}; + // cmd_frame = { + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, + // + // 0x0B, response[11], 0x0C, response[12], + // 0x0D, response[13], 0x0E, response[14], + // }; + // + // // 打印组的帧 + // for (size_t i = 0; i < cmd_frame.size(); ++i) { + // std::cout << std::hex + // << std::uppercase + // << std::setw(2) + // << std::setfill('0') + // << static_cast(cmd_frame[i]) + // << " "; + // } + // + // // 缓存起来，后面保存所有点的时候一起保存 + // CommandAttribute current_command; + // current_command.command = cmd_frame; + // current_command.enabled = true; + // _commandCaches[_current_node + ".command"] = current_command; + // }).detach(); +} + +void SlamWrapper::reset_arm_position() const { + _serialPortPtr->write(default_command); +} + +void SlamWrapper::send_gas_warning() const { + TcpService::getInstance().send_gas_warning(10000); +} + +void SlamWrapper::arm_turn_let_top() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_center() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x09, 0xd1, 0x07, 0xd9, 0x08, 0x11, 0x08, 0x36, 0x08, 0x2a, 0x08, 0x03, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_bottom() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0x1a, 0x07, 0x65, 0x08, 0x99, 0x07, 0xb8, 0x0a, 0x8d, 0x07, 0xdb, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_forward() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x98, 0x07, 0x6F, 0x06, 0x91, 0x07, 0xCD, 0x0A, 0x93, 0x08, 0x56, 0x14, 0xFA + }); +} + //casic end void SlamWrapper::qt_notice_handler(const void *message) { @@ -282,30 +355,30 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); 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; - const std::vector command = get_command(arrive_); + _current_node = atoi(str_.c_str()); + std::cout << "I arrived node " << _current_node << ". " << notice_ << std::endl; + const std::vector command = get_command(); if (command.empty()) { - std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + std::cout << "No command found for node " << _current_node << ". " << notice_ << std::endl; return; } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_, command] { + std::thread countdown_thread([this, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write(command); + _serialPortPtr->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 - TcpService::getInstance().update_node(arrive_); + TcpService::getInstance().update_node(_current_node); std::this_thread::sleep_for(std::chrono::seconds(5)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - serial_port_->write(default_command); + _serialPortPtr->write(default_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } @@ -410,6 +483,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } + // arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 0fec4c3..e438262 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,13 +58,30 @@ explicit SlamWrapper(); //casic start - void control_robotic_arm(); + void control_robotic_arm() const; - void move_to_i(); + void move_to_i() const; - void move_to_o(); + void move_to_o() const; - std::vector get_command(int index); + std::vector get_command() const; + + void arm_power_off() const; + + void arm_power_on() const; + + void reset_arm_position() const; + + void send_gas_warning() const; + + void arm_turn_let_top() const; + + void arm_turn_let_center() const; + + void arm_turn_let_bottom() const; + + void arm_turn_let_forward() const; + //casic end void qt_notice_handler(const void *message); @@ -102,11 +119,15 @@ private: int index = 0; const Odometry_ *currentOdom{}; - RoboticArmSerialPort *serial_port_; + + RoboticArmSerialPort *_serialPortPtr; 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 _armCommands; + + int _current_node = 0; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index e819793..22dc995 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -72,6 +72,30 @@ _slam.move_to_i(); } else if (received_string == "o") { _slam.move_to_o(); + } else if (received_string == "b") { + std::cout << "reset arm position" << std::endl; + _slam.reset_arm_position(); + } else if (received_string == "f") { + std::cout << "arm power off" << std::endl; + _slam.arm_power_off(); + } else if (received_string == "g") { + std::cout << "arm power on" << std::endl; + _slam.arm_power_on(); + } else if (received_string == "h") { + std::cout << "send gas warning" << std::endl; + _slam.send_gas_warning(); + } else if (received_string == "j") { + std::cout << "arm turn let top" << std::endl; + _slam.arm_turn_let_top(); + } else if (received_string == "k") { + std::cout << "arm turn let center" << std::endl; + _slam.arm_turn_let_center(); + } else if (received_string == "l") { + std::cout << "arm turn let bottom" << std::endl; + _slam.arm_turn_let_bottom(); + } else if (received_string == "m") { + std::cout << "arm turn let forward" << std::endl; + _slam.arm_turn_let_forward(); } else { std::cout << "Unknown command" << std::endl; } diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 713930c..32304e4 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -125,6 +125,20 @@ } } +void TcpService::send_gas_warning(const int value) { + std::stringstream ss; + ss << "2" << "," + << value << "," + << "116.296975" << "," + << "39.990889" + << std::endl; + const std::string message = ss.str(); + std::cout << "TcpService send gas warning: " << message << std::endl; + for (const auto &client_socket: _client_sockets) { + send(client_socket, message.c_str(), message.size(), 0); + } +} + void TcpService::update_gas_value(const int value) { gas_value = value; std::cout << "TcpService update gas value: " << gas_value << " ppm·m" << std::endl; diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index b9777e9..c7e2b70 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -71,8 +71,6 @@ // 上传服务器 if (Utils::is_network_reachable(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT)) { upload_to_server_async(result); - } else { - std::cout << "网络异常，上传甲烷数据到" << ConstantConfig::SERVICE_ADDRESS << "失败" << std::endl; } } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cecbe58..ac929f0 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -116,7 +116,7 @@ std::thread serial_port_thread([this] { //初始化机械臂串口 boost::asio::io_service io; - serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); + _serialPortPtr = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE); io.run(); }); serial_port_thread.detach(); @@ -150,7 +150,7 @@ if (end_bracket != std::string::npos) { if (!current_section.empty()) { // 将上一个节的数据存储到 map 中 - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } current_section = line.substr(1, end_bracket - 1); // 初始化新的节数据 @@ -182,15 +182,15 @@ } // 存储最后一个节的数据 if (!current_section.empty()) { - arm_commands_[current_section + ".command"] = current_command; + _armCommands[current_section + ".command"] = current_command; } config_file.close(); - std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; + std::cout << "SlamWrapper init success, has " << _armCommands.size() << " arm commands " << std::endl; } //casic start -void SlamWrapper::control_robotic_arm() { +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 }; @@ -199,15 +199,15 @@ }; std::cout << "control_robotic_arm start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(5)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "control_robotic_arm end" << std::endl; } -void SlamWrapper::move_to_i() { +void SlamWrapper::move_to_i() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA }; @@ -216,15 +216,15 @@ }; std::cout << "move_to_i start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(6)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(9)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_i end" << std::endl; } -void SlamWrapper::move_to_o() { +void SlamWrapper::move_to_o() const { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA }; @@ -233,25 +233,98 @@ }; std::cout << "move_to_o start" << std::endl; - serial_port_->write(left_command); + _serialPortPtr->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(right_command); + _serialPortPtr->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - serial_port_->write(default_command); + _serialPortPtr->write(default_command); std::cout << "move_to_o end" << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); +std::vector SlamWrapper::get_command() const { + const std::string section = std::to_string(_current_node); const std::string full_key = section + ".command"; - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { + const auto it = _armCommands.find(full_key); + if (it != _armCommands.end() && it->second.enabled) { return it->second.command; } return {}; } +void SlamWrapper::arm_power_off() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); +} + +void SlamWrapper::arm_power_on() const { + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // // 等待1秒 + // std::thread([this] { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // // 查询机械臂电位 + // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + // const auto response = _serialPortPtr->read_from_port(); + // // 组帧，如：0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA + // std::vector cmd_frame = {}; + // cmd_frame = { + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, + // + // 0x0B, response[11], 0x0C, response[12], + // 0x0D, response[13], 0x0E, response[14], + // }; + // + // // 打印组的帧 + // for (size_t i = 0; i < cmd_frame.size(); ++i) { + // std::cout << std::hex + // << std::uppercase + // << std::setw(2) + // << std::setfill('0') + // << static_cast(cmd_frame[i]) + // << " "; + // } + // + // // 缓存起来，后面保存所有点的时候一起保存 + // CommandAttribute current_command; + // current_command.command = cmd_frame; + // current_command.enabled = true; + // _commandCaches[_current_node + ".command"] = current_command; + // }).detach(); +} + +void SlamWrapper::reset_arm_position() const { + _serialPortPtr->write(default_command); +} + +void SlamWrapper::send_gas_warning() const { + TcpService::getInstance().send_gas_warning(10000); +} + +void SlamWrapper::arm_turn_let_top() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_center() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x09, 0xd1, 0x07, 0xd9, 0x08, 0x11, 0x08, 0x36, 0x08, 0x2a, 0x08, 0x03, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_bottom() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0x1a, 0x07, 0x65, 0x08, 0x99, 0x07, 0xb8, 0x0a, 0x8d, 0x07, 0xdb, 0x14, 0xFA + }); +} + +void SlamWrapper::arm_turn_let_forward() const { + _serialPortPtr->write({ + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x98, 0x07, 0x6F, 0x06, 0x91, 0x07, 0xCD, 0x0A, 0x93, 0x08, 0x56, 0x14, 0xFA + }); +} + //casic end void SlamWrapper::qt_notice_handler(const void *message) { @@ -282,30 +355,30 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); 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; - const std::vector command = get_command(arrive_); + _current_node = atoi(str_.c_str()); + std::cout << "I arrived node " << _current_node << ". " << notice_ << std::endl; + const std::vector command = get_command(); if (command.empty()) { - std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl; + std::cout << "No command found for node " << _current_node << ". " << notice_ << std::endl; return; } //发送暂停指令 pause_navigation(); //倒计时结束后恢复巡检 - std::thread countdown_thread([this,&arrive_, command] { + std::thread countdown_thread([this, command] { try { std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - serial_port_->write(command); + _serialPortPtr->write(command); std::this_thread::sleep_for(std::chrono::seconds(5)); //延迟5~10秒更新节点 - TcpService::getInstance().update_node(arrive_); + TcpService::getInstance().update_node(_current_node); std::this_thread::sleep_for(std::chrono::seconds(5)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - serial_port_->write(default_command); + _serialPortPtr->write(default_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } @@ -410,6 +483,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } + // arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 0fec4c3..e438262 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -58,13 +58,30 @@ explicit SlamWrapper(); //casic start - void control_robotic_arm(); + void control_robotic_arm() const; - void move_to_i(); + void move_to_i() const; - void move_to_o(); + void move_to_o() const; - std::vector get_command(int index); + std::vector get_command() const; + + void arm_power_off() const; + + void arm_power_on() const; + + void reset_arm_position() const; + + void send_gas_warning() const; + + void arm_turn_let_top() const; + + void arm_turn_let_center() const; + + void arm_turn_let_bottom() const; + + void arm_turn_let_forward() const; + //casic end void qt_notice_handler(const void *message); @@ -102,11 +119,15 @@ private: int index = 0; const Odometry_ *currentOdom{}; - RoboticArmSerialPort *serial_port_; + + RoboticArmSerialPort *_serialPortPtr; 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 _armCommands; + + int _current_node = 0; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index e819793..22dc995 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -72,6 +72,30 @@ _slam.move_to_i(); } else if (received_string == "o") { _slam.move_to_o(); + } else if (received_string == "b") { + std::cout << "reset arm position" << std::endl; + _slam.reset_arm_position(); + } else if (received_string == "f") { + std::cout << "arm power off" << std::endl; + _slam.arm_power_off(); + } else if (received_string == "g") { + std::cout << "arm power on" << std::endl; + _slam.arm_power_on(); + } else if (received_string == "h") { + std::cout << "send gas warning" << std::endl; + _slam.send_gas_warning(); + } else if (received_string == "j") { + std::cout << "arm turn let top" << std::endl; + _slam.arm_turn_let_top(); + } else if (received_string == "k") { + std::cout << "arm turn let center" << std::endl; + _slam.arm_turn_let_center(); + } else if (received_string == "l") { + std::cout << "arm turn let bottom" << std::endl; + _slam.arm_turn_let_bottom(); + } else if (received_string == "m") { + std::cout << "arm turn let forward" << std::endl; + _slam.arm_turn_let_forward(); } else { std::cout << "Unknown command" << std::endl; } diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 713930c..32304e4 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -125,6 +125,20 @@ } } +void TcpService::send_gas_warning(const int value) { + std::stringstream ss; + ss << "2" << "," + << value << "," + << "116.296975" << "," + << "39.990889" + << std::endl; + const std::string message = ss.str(); + std::cout << "TcpService send gas warning: " << message << std::endl; + for (const auto &client_socket: _client_sockets) { + send(client_socket, message.c_str(), message.size(), 0); + } +} + void TcpService::update_gas_value(const int value) { gas_value = value; std::cout << "TcpService update gas value: " << gas_value << " ppm·m" << std::endl; diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index 34a941a..822612a 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -35,6 +35,8 @@ */ void update_gas_value(int value); + void send_gas_warning(int value); + private: TcpService() = default;