Add video transmission sender.

rm_msgs/CMakeLists.txt

@@ -46,6 +46,7 @@ add_message_files(
         CustomControllerData.msg
         VTKeyboardMouseData.msg
         VTReceiverControlData.msg
+        VideoPacket.msg
 )
 
 add_service_files(

rm_msgs/msg/VideoPacket.msg

@@ -0,0 +1 @@
+uint8[300] data

rm_vt/include/rm_vt/video_transmission.h

@@ -7,13 +7,14 @@
 #include <ros/ros.h>
 
 #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<rm_msgs::CustomControllerData>("custom_controller_data", 1);
@@ -22,7 +23,6 @@ class VideoTransmission
     robot_custom_data_pub_ = nh.advertise<rm_msgs::RobotCustomData>("robot_custom_data", 1);
     robot_custom_data_2_pub_ = nh.advertise<rm_msgs::RobotCustomData2>("robot_custom_data_2", 1);
     custom_client_cmd_pub_ = nh.advertise<rm_msgs::CustomClientCmdData>("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<uint8_t> 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

rm_vt/include/rm_vt/video_transmission_sender.h

@@ -0,0 +1,45 @@
+//
+// Created by ljyi on 2026/4/6.
+//
+
+#pragma once
+
+#include <ros/ros.h>
+#include <cstring>
+#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

rm_vt/src/main.cpp

@@ -1,13 +1,19 @@
 //
 // Created by ch on 24-11-23.
 //
+#include <Eigen/Geometry>
 #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())
   {

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<bool>(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;

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<FrameHeader*>(tx_buffer);
+
+  frame_header->sof = 0xA5;
+  frame_header->data_length = data_len;
+  memcpy(&tx_buffer[k_header_length_], reinterpret_cast<uint8_t*>(&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<int>(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