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