diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index c6b850b..fce59b3 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -13,36 +13,26 @@ port_.set_option(serial_port_base::character_size(8)); port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - - read_from_port(); } -void RoboticArmSerialPort::read_from_port() { - async_read_until( - port_, buffer_, 0xFA, - [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable { - if (!error) { - // 获取接收到的数据 - std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); - is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) - ); +std::vector RoboticArmSerialPort::read_from_port(const std::vector &command) { + write(command); - std::cout << "received " << bytes_transferred << " bytes: "; - for (size_t i = 0; i < bytes_transferred; ++i) { - std::cout << std::hex - << std::uppercase - << std::setw(2) - << std::setfill('0') - << static_cast(received_data[i]) - << " "; - } - std::cout << std::endl; - } else { - std::cerr << "error: " << error.message() << std::endl; - } - }); + boost::asio::streambuf buffer; + boost::system::error_code ec; + // 同步读取直到遇到 0xFA 或超时 + const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec); + + if (ec) { + std::cerr << "Read error: " << ec.message() << std::endl; + return {}; + } + + // 提取数据 + std::vector frame(bytes_transferred); + std::istream is(&buffer); + is.read(reinterpret_cast(frame.data()), static_cast(bytes_transferred)); + return frame; } void RoboticArmSerialPort::write(std::vector command) { @@ -56,4 +46,4 @@ } std::cout << std::endl; boost::asio::write(port_, boost::asio::buffer(command)); -} \ No newline at end of file +} diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index c6b850b..fce59b3 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -13,36 +13,26 @@ port_.set_option(serial_port_base::character_size(8)); port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - - read_from_port(); } -void RoboticArmSerialPort::read_from_port() { - async_read_until( - port_, buffer_, 0xFA, - [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable { - if (!error) { - // 获取接收到的数据 - std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); - is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) - ); +std::vector RoboticArmSerialPort::read_from_port(const std::vector &command) { + write(command); - std::cout << "received " << bytes_transferred << " bytes: "; - for (size_t i = 0; i < bytes_transferred; ++i) { - std::cout << std::hex - << std::uppercase - << std::setw(2) - << std::setfill('0') - << static_cast(received_data[i]) - << " "; - } - std::cout << std::endl; - } else { - std::cerr << "error: " << error.message() << std::endl; - } - }); + boost::asio::streambuf buffer; + boost::system::error_code ec; + // 同步读取直到遇到 0xFA 或超时 + const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec); + + if (ec) { + std::cerr << "Read error: " << ec.message() << std::endl; + return {}; + } + + // 提取数据 + std::vector frame(bytes_transferred); + std::istream is(&buffer); + is.read(reinterpret_cast(frame.data()), static_cast(bytes_transferred)); + return frame; } void RoboticArmSerialPort::write(std::vector command) { @@ -56,4 +46,4 @@ } std::cout << std::endl; boost::asio::write(port_, boost::asio::buffer(command)); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index ac77fc6..d93d543 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -17,12 +17,12 @@ void write(std::vector command); + std::vector read_from_port(const std::vector &command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; boost::asio::streambuf buffer_; - - void read_from_port(); }; diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index c6b850b..fce59b3 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -13,36 +13,26 @@ port_.set_option(serial_port_base::character_size(8)); port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - - read_from_port(); } -void RoboticArmSerialPort::read_from_port() { - async_read_until( - port_, buffer_, 0xFA, - [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable { - if (!error) { - // 获取接收到的数据 - std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); - is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) - ); +std::vector RoboticArmSerialPort::read_from_port(const std::vector &command) { + write(command); - std::cout << "received " << bytes_transferred << " bytes: "; - for (size_t i = 0; i < bytes_transferred; ++i) { - std::cout << std::hex - << std::uppercase - << std::setw(2) - << std::setfill('0') - << static_cast(received_data[i]) - << " "; - } - std::cout << std::endl; - } else { - std::cerr << "error: " << error.message() << std::endl; - } - }); + boost::asio::streambuf buffer; + boost::system::error_code ec; + // 同步读取直到遇到 0xFA 或超时 + const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec); + + if (ec) { + std::cerr << "Read error: " << ec.message() << std::endl; + return {}; + } + + // 提取数据 + std::vector frame(bytes_transferred); + std::istream is(&buffer); + is.read(reinterpret_cast(frame.data()), static_cast(bytes_transferred)); + return frame; } void RoboticArmSerialPort::write(std::vector command) { @@ -56,4 +46,4 @@ } std::cout << std::endl; boost::asio::write(port_, boost::asio::buffer(command)); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index ac77fc6..d93d543 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -17,12 +17,12 @@ void write(std::vector command); + std::vector read_from_port(const std::vector &command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; boost::asio::streambuf buffer_; - - void read_from_port(); }; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ac929f0..b02899f 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -253,44 +253,92 @@ } void SlamWrapper::arm_power_off() const { - // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); + _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::arm_power_on() { + _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // 等待1秒 + std::this_thread::sleep_for(std::chrono::seconds(1)); + // 查询机械臂电位 + const auto response = _serialPortPtr->read_from_port({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + if (!response.empty()) { + std::cout << "Recv frame: "; + for (const auto &b: response) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + //Received frame (11 bytes): FE FE 0E 3D 0A 9A 07 75 06 92 07 D7 0A 9A 08 65 FA + std::cout << std::endl; + + // 组帧 + std::vector cmd_frame = {}; + cmd_frame = { + 0xFE, 0xFE, 0x0F, 0x3C, + response[4], response[5], // 一号舵机高低位 + response[6], response[7], // 二号舵机高低位 + response[8], response[9], // 三号舵机高低位 + response[10], response[11], // 四号舵机高低位 + response[12], response[13], // 五号舵机高低位 + response[14], response[15], // 六号舵机高低位 + 0x1E, // 舵机转动速度 + 0xFA + }; + + // 打印组的帧 + std::cout << "Send frame: "; + for (const auto &b: cmd_frame) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + std::cout << std::endl; + + // 缓存起来，后面保存所有点的时候一起保存 + CommandAttribute current_command; + current_command.command = cmd_frame; + current_command.enabled = _current_node == 2; + _commandCaches[_current_node] = current_command; + } else { + std::cerr << "No data received." << std::endl; + } +} + +void SlamWrapper::save_command() const { + if (!_commandCaches.empty()) { + //覆盖保存/home/unitree/Desktop/robotic_arm_commands.conf + const std::string filepath = ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG; + std::ofstream file(filepath); + if (!file.is_open()) { + std::cerr << "Failed to open config file for writing: " << filepath << std::endl; + return; + } + for (const auto &[node, attr]: _commandCaches) { + // 写入节名 + file << "[" << node << "]" << std::endl; + + // 写入 command 字段 + file << "command="; + for (size_t i = 0; i < attr.command.size(); ++i) { + file << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(attr.command[i]); + if (i != attr.command.size() - 1) { + file << " "; + } + } + file << std::endl; + + // 写入 enabled 字段 + file << "enabled=" << (attr.enabled ? "1" : "0") << std::endl; + + // 空行分隔 + file << std::endl; + } + file.close(); + std::cout << "Command saved to config file: " << filepath << std::endl; + } else { + std::cout << "No command to save." << std::endl; + } } void SlamWrapper::reset_arm_position() const { @@ -483,7 +531,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } - // arm_power_off(); + arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { @@ -538,6 +586,7 @@ edgeMsg.edge_().edge_state_4_().push_back(0); } pubQtEdge->Write(edgeMsg); + save_command(); } void SlamWrapper::close_all_node() { diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index c6b850b..fce59b3 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -13,36 +13,26 @@ port_.set_option(serial_port_base::character_size(8)); port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - - read_from_port(); } -void RoboticArmSerialPort::read_from_port() { - async_read_until( - port_, buffer_, 0xFA, - [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable { - if (!error) { - // 获取接收到的数据 - std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); - is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) - ); +std::vector RoboticArmSerialPort::read_from_port(const std::vector &command) { + write(command); - std::cout << "received " << bytes_transferred << " bytes: "; - for (size_t i = 0; i < bytes_transferred; ++i) { - std::cout << std::hex - << std::uppercase - << std::setw(2) - << std::setfill('0') - << static_cast(received_data[i]) - << " "; - } - std::cout << std::endl; - } else { - std::cerr << "error: " << error.message() << std::endl; - } - }); + boost::asio::streambuf buffer; + boost::system::error_code ec; + // 同步读取直到遇到 0xFA 或超时 + const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec); + + if (ec) { + std::cerr << "Read error: " << ec.message() << std::endl; + return {}; + } + + // 提取数据 + std::vector frame(bytes_transferred); + std::istream is(&buffer); + is.read(reinterpret_cast(frame.data()), static_cast(bytes_transferred)); + return frame; } void RoboticArmSerialPort::write(std::vector command) { @@ -56,4 +46,4 @@ } std::cout << std::endl; boost::asio::write(port_, boost::asio::buffer(command)); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index ac77fc6..d93d543 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -17,12 +17,12 @@ void write(std::vector command); + std::vector read_from_port(const std::vector &command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; boost::asio::streambuf buffer_; - - void read_from_port(); }; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ac929f0..b02899f 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -253,44 +253,92 @@ } void SlamWrapper::arm_power_off() const { - // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); + _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::arm_power_on() { + _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // 等待1秒 + std::this_thread::sleep_for(std::chrono::seconds(1)); + // 查询机械臂电位 + const auto response = _serialPortPtr->read_from_port({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + if (!response.empty()) { + std::cout << "Recv frame: "; + for (const auto &b: response) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + //Received frame (11 bytes): FE FE 0E 3D 0A 9A 07 75 06 92 07 D7 0A 9A 08 65 FA + std::cout << std::endl; + + // 组帧 + std::vector cmd_frame = {}; + cmd_frame = { + 0xFE, 0xFE, 0x0F, 0x3C, + response[4], response[5], // 一号舵机高低位 + response[6], response[7], // 二号舵机高低位 + response[8], response[9], // 三号舵机高低位 + response[10], response[11], // 四号舵机高低位 + response[12], response[13], // 五号舵机高低位 + response[14], response[15], // 六号舵机高低位 + 0x1E, // 舵机转动速度 + 0xFA + }; + + // 打印组的帧 + std::cout << "Send frame: "; + for (const auto &b: cmd_frame) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + std::cout << std::endl; + + // 缓存起来，后面保存所有点的时候一起保存 + CommandAttribute current_command; + current_command.command = cmd_frame; + current_command.enabled = _current_node == 2; + _commandCaches[_current_node] = current_command; + } else { + std::cerr << "No data received." << std::endl; + } +} + +void SlamWrapper::save_command() const { + if (!_commandCaches.empty()) { + //覆盖保存/home/unitree/Desktop/robotic_arm_commands.conf + const std::string filepath = ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG; + std::ofstream file(filepath); + if (!file.is_open()) { + std::cerr << "Failed to open config file for writing: " << filepath << std::endl; + return; + } + for (const auto &[node, attr]: _commandCaches) { + // 写入节名 + file << "[" << node << "]" << std::endl; + + // 写入 command 字段 + file << "command="; + for (size_t i = 0; i < attr.command.size(); ++i) { + file << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(attr.command[i]); + if (i != attr.command.size() - 1) { + file << " "; + } + } + file << std::endl; + + // 写入 enabled 字段 + file << "enabled=" << (attr.enabled ? "1" : "0") << std::endl; + + // 空行分隔 + file << std::endl; + } + file.close(); + std::cout << "Command saved to config file: " << filepath << std::endl; + } else { + std::cout << "No command to save." << std::endl; + } } void SlamWrapper::reset_arm_position() const { @@ -483,7 +531,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } - // arm_power_off(); + arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { @@ -538,6 +586,7 @@ edgeMsg.edge_().edge_state_4_().push_back(0); } pubQtEdge->Write(edgeMsg); + save_command(); } void SlamWrapper::close_all_node() { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index e438262..d8d4546 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -68,7 +68,9 @@ void arm_power_off() const; - void arm_power_on() const; + void arm_power_on(); + + void save_command() const; void reset_arm_position() const; @@ -127,7 +129,7 @@ std::map _armCommands; int _current_node = 0; - std::map _commandCaches; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/robotic_arm_commands.conf b/robotic_arm_commands.conf index 0fbc584..ad95d19 100644 --- a/robotic_arm_commands.conf +++ b/robotic_arm_commands.conf @@ -1,11 +1,11 @@ [1] -command=FE FE 0F 3C 05 1F 09 70 05 04 08 67 08 6E 07 DF 1E FA +command=FE FE 0F 3C 05 B3 08 19 07 5E 08 73 08 1D 08 A6 14 FA enabled=0 [2] -command=FE FE 0F 3C 0A 40 0A 89 03 DE 08 43 09 A8 07 E1 1E FA +command=FE FE 0F 3C 0A 98 07 6F 06 91 07 CD 0A 93 08 56 14 FA enabled=1 [3] -command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 1E FA +command=FE FE 0F 3C 07 F7 08 18 08 0B 08 4B 0A 7D 08 0A 14 FA enabled=0 \ No newline at end of file diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index c6b850b..fce59b3 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -13,36 +13,26 @@ port_.set_option(serial_port_base::character_size(8)); port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - - read_from_port(); } -void RoboticArmSerialPort::read_from_port() { - async_read_until( - port_, buffer_, 0xFA, - [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable { - if (!error) { - // 获取接收到的数据 - std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); - is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) - ); +std::vector RoboticArmSerialPort::read_from_port(const std::vector &command) { + write(command); - std::cout << "received " << bytes_transferred << " bytes: "; - for (size_t i = 0; i < bytes_transferred; ++i) { - std::cout << std::hex - << std::uppercase - << std::setw(2) - << std::setfill('0') - << static_cast(received_data[i]) - << " "; - } - std::cout << std::endl; - } else { - std::cerr << "error: " << error.message() << std::endl; - } - }); + boost::asio::streambuf buffer; + boost::system::error_code ec; + // 同步读取直到遇到 0xFA 或超时 + const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec); + + if (ec) { + std::cerr << "Read error: " << ec.message() << std::endl; + return {}; + } + + // 提取数据 + std::vector frame(bytes_transferred); + std::istream is(&buffer); + is.read(reinterpret_cast(frame.data()), static_cast(bytes_transferred)); + return frame; } void RoboticArmSerialPort::write(std::vector command) { @@ -56,4 +46,4 @@ } std::cout << std::endl; boost::asio::write(port_, boost::asio::buffer(command)); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index ac77fc6..d93d543 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -17,12 +17,12 @@ void write(std::vector command); + std::vector read_from_port(const std::vector &command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; boost::asio::streambuf buffer_; - - void read_from_port(); }; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ac929f0..b02899f 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -253,44 +253,92 @@ } void SlamWrapper::arm_power_off() const { - // _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x13, 0xFA}); + _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::arm_power_on() { + _serialPortPtr->write({0xFE, 0xFE, 0x02, 0x10, 0xFA}); + // 等待1秒 + std::this_thread::sleep_for(std::chrono::seconds(1)); + // 查询机械臂电位 + const auto response = _serialPortPtr->read_from_port({0xFE, 0xFE, 0x02, 0x3D, 0xFA}); + if (!response.empty()) { + std::cout << "Recv frame: "; + for (const auto &b: response) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + //Received frame (11 bytes): FE FE 0E 3D 0A 9A 07 75 06 92 07 D7 0A 9A 08 65 FA + std::cout << std::endl; + + // 组帧 + std::vector cmd_frame = {}; + cmd_frame = { + 0xFE, 0xFE, 0x0F, 0x3C, + response[4], response[5], // 一号舵机高低位 + response[6], response[7], // 二号舵机高低位 + response[8], response[9], // 三号舵机高低位 + response[10], response[11], // 四号舵机高低位 + response[12], response[13], // 五号舵机高低位 + response[14], response[15], // 六号舵机高低位 + 0x1E, // 舵机转动速度 + 0xFA + }; + + // 打印组的帧 + std::cout << "Send frame: "; + for (const auto &b: cmd_frame) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(b) << " "; + } + std::cout << std::endl; + + // 缓存起来，后面保存所有点的时候一起保存 + CommandAttribute current_command; + current_command.command = cmd_frame; + current_command.enabled = _current_node == 2; + _commandCaches[_current_node] = current_command; + } else { + std::cerr << "No data received." << std::endl; + } +} + +void SlamWrapper::save_command() const { + if (!_commandCaches.empty()) { + //覆盖保存/home/unitree/Desktop/robotic_arm_commands.conf + const std::string filepath = ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG; + std::ofstream file(filepath); + if (!file.is_open()) { + std::cerr << "Failed to open config file for writing: " << filepath << std::endl; + return; + } + for (const auto &[node, attr]: _commandCaches) { + // 写入节名 + file << "[" << node << "]" << std::endl; + + // 写入 command 字段 + file << "command="; + for (size_t i = 0; i < attr.command.size(); ++i) { + file << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(attr.command[i]); + if (i != attr.command.size() - 1) { + file << " "; + } + } + file << std::endl; + + // 写入 enabled 字段 + file << "enabled=" << (attr.enabled ? "1" : "0") << std::endl; + + // 空行分隔 + file << std::endl; + } + file.close(); + std::cout << "Command saved to config file: " << filepath << std::endl; + } else { + std::cout << "No command to save." << std::endl; + } } void SlamWrapper::reset_arm_position() const { @@ -483,7 +531,7 @@ if (node_name >= 2) { add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes } - // arm_power_off(); + arm_power_off(); } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { @@ -538,6 +586,7 @@ edgeMsg.edge_().edge_state_4_().push_back(0); } pubQtEdge->Write(edgeMsg); + save_command(); } void SlamWrapper::close_all_node() { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index e438262..d8d4546 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -68,7 +68,9 @@ void arm_power_off() const; - void arm_power_on() const; + void arm_power_on(); + + void save_command() const; void reset_arm_position() const; @@ -127,7 +129,7 @@ std::map _armCommands; int _current_node = 0; - std::map _commandCaches; + std::map _commandCaches; // pub ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 22dc995..a4f508e 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -70,8 +70,10 @@ _slam.control_robotic_arm(); } else if (received_string == "i") { _slam.move_to_i(); + // _slam.arm_power_on(); } else if (received_string == "o") { _slam.move_to_o(); + // _slam.save_command(); } else if (received_string == "b") { std::cout << "reset arm position" << std::endl; _slam.reset_arm_position();