From 85200054dcf7a39511ebd4f4ca1972f7e83d191c Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Wed, 8 Apr 2026 01:01:25 +0800 Subject: [PATCH 1/2] Add vt sender for test. --- .../include/rm_vt/video_transmission_sender.h | 35 +++++++++++ rm_vt/src/main.cpp | 10 +++- rm_vt/src/video_transmission_sender.cpp | 58 +++++++++++++++++++ 3 files changed, 101 insertions(+), 2 deletions(-) create mode 100644 rm_vt/include/rm_vt/video_transmission_sender.h create mode 100644 rm_vt/src/video_transmission_sender.cpp diff --git a/rm_vt/include/rm_vt/video_transmission_sender.h b/rm_vt/include/rm_vt/video_transmission_sender.h new file mode 100644 index 00000000..77fc1fed --- /dev/null +++ b/rm_vt/include/rm_vt/video_transmission_sender.h @@ -0,0 +1,35 @@ +// +// Created by ljyi on 2026/4/6. +// + +#pragma once + +#include +#include "common/data.h" +#include "common/protocol.h" +#include "rm_msgs/VideoPacket.h" + +namespace rm_vt +{ +class VideoTransmissionBase +{ +public: + explicit VideoTransmissionBase(ros::NodeHandle& nh, Base& base); + virtual ~VideoTransmissionBase() = default; + +private: + void videoStreamCB(const rm_msgs::VideoPacketConstPtr& msg); + void sendVideoPacket(const uint8_t* data, size_t data_len); + void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int data_len); + void clearTxBuffer(); + + Base& base_; + ros::Subscriber deploy_video_sub_; + uint8_t tx_buffer_[309]{}; + int tx_len_{}; + ros::Timer send_serial_data_timer_; + + static const int k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; +}; + +} // namespace rm_vt diff --git a/rm_vt/src/main.cpp b/rm_vt/src/main.cpp index 3f4db47b..8c79cf45 100644 --- a/rm_vt/src/main.cpp +++ b/rm_vt/src/main.cpp @@ -1,18 +1,24 @@ // // Created by ch on 24-11-23. // +#include +#include "rm_vt/common/data.h" #include "rm_vt/video_transmission.h" +#include "rm_vt/video_transmission_sender.h" int main(int argc, char** argv) { ros::init(argc, argv, "rm_vt"); ros::NodeHandle nh("~"); - rm_vt::VideoTransmission video_transmission(nh); + // rm_vt::VideoTransmission video_transmission(nh); + rm_vt::Base base_; + base_.initSerial(); + rm_vt::VideoTransmissionBase video_transmission_sender(nh, base_); ros::Rate loop_rate(100); while (ros::ok()) { ros::spinOnce(); - video_transmission.read(); + // video_transmission.read(); loop_rate.sleep(); } } diff --git a/rm_vt/src/video_transmission_sender.cpp b/rm_vt/src/video_transmission_sender.cpp new file mode 100644 index 00000000..16959845 --- /dev/null +++ b/rm_vt/src/video_transmission_sender.cpp @@ -0,0 +1,58 @@ +// +// Created by ljyi on 2026/4/6. +// + +#include "rm_vt/video_transmission_sender.h" +#include + +namespace rm_vt +{ +VideoTransmissionBase::VideoTransmissionBase(ros::NodeHandle& nh, Base& base) : base_(base), tx_len_(0) +{ + deploy_video_sub_ = nh.subscribe("/video_stream", 10, &VideoTransmissionBase::videoStreamCB, this); +} + +void VideoTransmissionBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int data_len) +{ + memset(tx_buffer, 0, k_header_length_ + k_cmd_id_length_ + data_len + k_tail_length_); + auto* frame_header = reinterpret_cast(tx_buffer); + frame_header->sof = 0xA5; + frame_header->data_length = data_len; + memcpy(tx_buffer + k_header_length_, reinterpret_cast(&cmd_id), k_cmd_id_length_); + base_.appendCRC8CheckSum(tx_buffer, k_header_length_); + memcpy(tx_buffer + k_header_length_ + k_cmd_id_length_, data, data_len); + base_.appendCRC16CheckSum(tx_buffer, k_header_length_ + k_cmd_id_length_ + data_len + k_tail_length_); +} + +void VideoTransmissionBase::clearTxBuffer() +{ + memset(tx_buffer_, 0, sizeof(tx_buffer_)); + tx_len_ = 0; +} + +void VideoTransmissionBase::sendVideoPacket(const uint8_t* data, size_t data_len) +{ + uint8_t tx_data[sizeof(rm_vt::RobotToCustomData2)] = { 0 }; + auto* video_data = reinterpret_cast(tx_data); + memcpy(video_data->data, data, data_len); + int total_len = k_header_length_ + k_cmd_id_length_ + sizeof(rm_vt::RobotToCustomData2) + k_tail_length_; + for (int i = 0; i < total_len; ++i) + tx_buffer_[i] = 0; + pack(tx_buffer_, tx_data, ROBOT_TO_CUSTOM_CLIENT_CMD, sizeof(rm_vt::RobotToCustomData2)); + try + { + base_.serial_.write(tx_buffer_, total_len); + } + catch (serial::PortNotOpenedException& e) + { + ROS_ERROR_STREAM(e.what()); + } + clearTxBuffer(); +} + +void VideoTransmissionBase::videoStreamCB(const rm_msgs::VideoPacketConstPtr& msg) +{ + sendVideoPacket(msg->data.data(), 300); +} + +} // namespace rm_vt From 33d26216d35462b046633e5ef2085f0944da2287 Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Sat, 18 Apr 2026 18:41:58 +0800 Subject: [PATCH 2/2] Fix logic error. --- rm_msgs/CMakeLists.txt | 1 + rm_msgs/msg/VideoPacket.msg | 1 + rm_vt/include/rm_vt/video_transmission.h | 14 ++--- .../include/rm_vt/video_transmission_sender.h | 45 +++++++++++++++ rm_vt/src/main.cpp | 8 ++- rm_vt/src/video_transmission.cpp | 4 +- rm_vt/src/video_transmission_sender.cpp | 55 +++++++++++++++++++ 7 files changed, 118 insertions(+), 10 deletions(-) create mode 100644 rm_msgs/msg/VideoPacket.msg create mode 100644 rm_vt/include/rm_vt/video_transmission_sender.h create mode 100644 rm_vt/src/video_transmission_sender.cpp diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index ca956c27..db423abb 100755 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -46,6 +46,7 @@ add_message_files( CustomControllerData.msg VTKeyboardMouseData.msg VTReceiverControlData.msg + VideoPacket.msg ) add_service_files( diff --git a/rm_msgs/msg/VideoPacket.msg b/rm_msgs/msg/VideoPacket.msg new file mode 100644 index 00000000..82e43ee5 --- /dev/null +++ b/rm_msgs/msg/VideoPacket.msg @@ -0,0 +1 @@ +uint8[300] data diff --git a/rm_vt/include/rm_vt/video_transmission.h b/rm_vt/include/rm_vt/video_transmission.h index e13381d0..f60effef 100644 --- a/rm_vt/include/rm_vt/video_transmission.h +++ b/rm_vt/include/rm_vt/video_transmission.h @@ -7,13 +7,14 @@ #include #include "rm_vt/common/data.h" +#include "rm_vt/video_transmission_sender.h" namespace rm_vt { class VideoTransmission { public: - explicit VideoTransmission(ros::NodeHandle& nh) : last_get_data_time_(ros::Time::now()) + explicit VideoTransmission(ros::NodeHandle& nh, Base& base) : base_(base), last_get_data_time_(ros::Time::now()) { ROS_INFO("Video transmission load."); custom_controller_cmd_pub_ = nh.advertise("custom_controller_data", 1); @@ -22,7 +23,6 @@ class VideoTransmission robot_custom_data_pub_ = nh.advertise("robot_custom_data", 1); robot_custom_data_2_pub_ = nh.advertise("robot_custom_data_2", 1); custom_client_cmd_pub_ = nh.advertise("custom_client_cmd_data", 1); - base_.initSerial(); } void read(); void clearRxBuffer() @@ -34,16 +34,16 @@ class VideoTransmission ros::Publisher custom_controller_cmd_pub_, vt_keyboard_mouse_pub_, vt_receiver_control_pub_; ros::Publisher robot_custom_data_pub_, robot_custom_data_2_pub_, custom_client_cmd_pub_; - Base base_; + Base& base_; std::vector rx_buffer_; - int rx_len_; + int rx_len_{}; private: int unpack(uint8_t* rx_data); int control_data_unpack(uint8_t* rx_data); ros::Time last_get_data_time_; - const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; - const int k_unpack_buffer_length_ = 256; - uint8_t unpack_buffer_[256]{}; + const int k_frame_length_ = 309, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; + const int k_unpack_buffer_length_ = 512; + uint8_t unpack_buffer_[512]{}; }; } // namespace rm_vt diff --git a/rm_vt/include/rm_vt/video_transmission_sender.h b/rm_vt/include/rm_vt/video_transmission_sender.h new file mode 100644 index 00000000..8cf2295d --- /dev/null +++ b/rm_vt/include/rm_vt/video_transmission_sender.h @@ -0,0 +1,45 @@ +// +// Created by ljyi on 2026/4/6. +// + +#pragma once + +#include +#include +#include "common/data.h" +#include "common/protocol.h" +#include "rm_msgs/VideoPacket.h" + +namespace rm_vt +{ +class VideoTransmissionSender +{ +public: + explicit VideoTransmissionSender(ros::NodeHandle& nh, Base& base) : base_(base) + { + ROS_INFO("Video transmission sender load."); + deploy_video_stream_sub_ = nh.subscribe("/video_stream", 10, &VideoTransmissionSender::deployVideoStreamCB, this); + } + virtual ~VideoTransmissionSender() = default; + +private: + void deployVideoStreamCB(const rm_msgs::VideoPacketConstPtr& msg); + void sendDeployVideoStream(const uint8_t* data); + void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len); + void clearTxBuffer() + { + for (int i = 0; i < k_frame_length_; i++) + tx_buffer_[i] = 0; + tx_len_ = 0; + } + + ros::Subscriber deploy_video_stream_sub_; + ros::Time deploy_video_stream_last_send_; + Base& base_; + uint8_t tx_buffer_[309]{}; + int tx_len_{}; + + const int k_frame_length_ = 309, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; +}; + +} // namespace rm_vt diff --git a/rm_vt/src/main.cpp b/rm_vt/src/main.cpp index 3f4db47b..c71647dd 100644 --- a/rm_vt/src/main.cpp +++ b/rm_vt/src/main.cpp @@ -1,13 +1,19 @@ // // Created by ch on 24-11-23. // +#include #include "rm_vt/video_transmission.h" +#include "rm_vt/video_transmission_sender.h" +#include "rm_vt/common/data.h" int main(int argc, char** argv) { ros::init(argc, argv, "rm_vt"); ros::NodeHandle nh("~"); - rm_vt::VideoTransmission video_transmission(nh); + rm_vt::Base base; + base.initSerial(); + rm_vt::VideoTransmission video_transmission(nh, base); + rm_vt::VideoTransmissionSender video_transmission_sender(nh, base); ros::Rate loop_rate(100); while (ros::ok()) { diff --git a/rm_vt/src/video_transmission.cpp b/rm_vt/src/video_transmission.cpp index 246b1d00..b14a3537 100644 --- a/rm_vt/src/video_transmission.cpp +++ b/rm_vt/src/video_transmission.cpp @@ -14,7 +14,7 @@ void VideoTransmission::read() } else return; - uint8_t temp_buffer[256] = { 0 }; + uint8_t temp_buffer[512] = { 0 }; int frame_len; if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) base_.video_transmission_is_online_ = false; @@ -54,7 +54,7 @@ int VideoTransmission::unpack(uint8_t* rx_data) memcpy(&frame_header, rx_data, k_header_length_); if (static_cast(base_.verifyCRC8CheckSum(rx_data, k_header_length_))) { - if (frame_header.data_length > 256) // temporary and inaccurate value + if (frame_header.data_length > 512) // temporary and inaccurate value { ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length); return 0; diff --git a/rm_vt/src/video_transmission_sender.cpp b/rm_vt/src/video_transmission_sender.cpp new file mode 100644 index 00000000..1b7498a1 --- /dev/null +++ b/rm_vt/src/video_transmission_sender.cpp @@ -0,0 +1,55 @@ +// +// Created by ljyi on 2026/4/6. +// + +#include "rm_vt/video_transmission_sender.h" + +namespace rm_vt +{ +void VideoTransmissionSender::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int data_len) +{ + memset(tx_buffer, 0, k_frame_length_); + auto* frame_header = reinterpret_cast(tx_buffer); + + frame_header->sof = 0xA5; + frame_header->data_length = data_len; + memcpy(&tx_buffer[k_header_length_], reinterpret_cast(&cmd_id), k_cmd_id_length_); + base_.appendCRC8CheckSum(tx_buffer, k_header_length_); + memcpy(&tx_buffer[k_header_length_ + k_cmd_id_length_], data, data_len); + base_.appendCRC16CheckSum(tx_buffer, k_header_length_ + k_cmd_id_length_ + data_len + k_tail_length_); +} + +void VideoTransmissionSender::sendDeployVideoStream(const uint8_t* data) +{ + uint8_t tx_data[sizeof(rm_vt::RobotToCustomData2)] = { 0 }; + auto video_data = (rm_vt::RobotToCustomData2*)tx_data; + + for (int i = 0; i < 308; i++) + tx_buffer_[i] = 0; + for (int i = 0; i < 300; i++) + video_data->data[i] = data[i]; + pack(tx_buffer_, tx_data, rm_vt::ROBOT_TO_CUSTOM_CLIENT_CMD, sizeof(rm_vt::RobotToCustomData2)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_vt::RobotToCustomData2) + k_tail_length_); + try + { + base_.serial_.write(tx_buffer_, tx_len_); + } + catch (serial::PortNotOpenedException& e) + { + ROS_ERROR_STREAM(e.what()); + } + clearTxBuffer(); +} + +void VideoTransmissionSender::deployVideoStreamCB(const rm_msgs::VideoPacketConstPtr& msg) +{ + if (ros::Time::now() - deploy_video_stream_last_send_ <= ros::Duration(0.02)) + return; + else + { + sendDeployVideoStream(msg->data.data()); + deploy_video_stream_last_send_ = ros::Time::now(); + } +} + +} // namespace rm_vt