video_transmission.cpp
Go to the documentation of this file.
1 //
2 // Created by ch on 24-11-23.
3 //
4 #include "rm_vt/video_transmission.h"
5 
6 namespace rm_vt
7 {
8 void VideoTransmission::read()
9 {
10  if (base_.serial_.available())
11  {
12  rx_len_ = static_cast<int>(base_.serial_.available());
13  base_.serial_.read(rx_buffer_, rx_len_);
14  }
15  else
16  return;
17  uint8_t temp_buffer[256] = { 0 };
18  int frame_len;
19  if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1))
20  base_.video_transmission_is_online_ = false;
21  if (rx_len_ < k_unpack_buffer_length_)
22  {
23  for (int k_i = 0; k_i < k_unpack_buffer_length_ - rx_len_; ++k_i)
24  temp_buffer[k_i] = unpack_buffer_[k_i + rx_len_];
25  for (int k_i = 0; k_i < rx_len_; ++k_i)
26  temp_buffer[k_i + k_unpack_buffer_length_ - rx_len_] = rx_buffer_[k_i];
27  for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i)
28  unpack_buffer_[k_i] = temp_buffer[k_i];
29  }
30  for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i)
31  {
32  if (unpack_buffer_[k_i] == 0xA5)
33  {
34  frame_len = unpack(&unpack_buffer_[k_i]);
35  if (frame_len != -1)
36  k_i += frame_len;
37  }
38  if (unpack_buffer_[k_i] == 0xA9 && unpack_buffer_[k_i + 1] == 0x53)
39  {
40  frame_len = control_data_unpack(&unpack_buffer_[k_i]);
41  if (frame_len != -1)
42  k_i += frame_len;
43  }
44  }
45  clearRxBuffer();
46 }
47 
48 int VideoTransmission::unpack(uint8_t* rx_data)
49 {
50  uint16_t cmd_id;
51  int frame_len;
52  rm_vt::FrameHeader frame_header;
53 
54  memcpy(&frame_header, rx_data, k_header_length_);
55  if (static_cast<bool>(base_.verifyCRC8CheckSum(rx_data, k_header_length_)))
56  {
57  if (frame_header.data_length > 256) // temporary and inaccurate value
58  {
59  ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length);
60  return 0;
61  }
62  frame_len = frame_header.data_length + k_header_length_ + k_cmd_id_length_ + k_tail_length_;
63  if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1)
64  {
65  cmd_id = (rx_data[6] << 8 | rx_data[5]);
66  switch (cmd_id)
67  {
68  case rm_vt::CUSTOM_CONTROLLER_CMD:
69  {
70  rm_vt::CustomControllerData custom_controller_ref;
71  rm_msgs::CustomControllerData custom_controller_data;
72  memcpy(&custom_controller_ref, rx_data + 7, sizeof(rm_vt::CustomControllerData));
73  custom_controller_data.encoder_data[0] = 3.14 *
74  ((uint16_t)(custom_controller_ref.encoder1_data[0] << 8) |
75  (uint16_t)custom_controller_ref.encoder1_data[1]) /
76  18000.0;
77  custom_controller_data.encoder_data[1] = 3.14 *
78  ((uint16_t)(custom_controller_ref.encoder2_data[0] << 8) |
79  (uint16_t)custom_controller_ref.encoder2_data[1]) /
80  18000.0;
81  custom_controller_data.encoder_data[5] = 3.14 *
82  ((uint16_t)(custom_controller_ref.encoder3_data[0] << 8) |
83  (uint16_t)custom_controller_ref.encoder3_data[1]) /
84  18000.0;
85  custom_controller_data.encoder_data[3] = 3.14 *
86  ((uint16_t)(custom_controller_ref.encoder4_data[0] << 8) |
87  (uint16_t)custom_controller_ref.encoder4_data[1]) /
88  18000.0;
89  custom_controller_data.encoder_data[4] = 3.14 *
90  ((uint16_t)(custom_controller_ref.encoder5_data[0] << 8) |
91  (uint16_t)custom_controller_ref.encoder5_data[1]) /
92  18000.0;
93  custom_controller_data.encoder_data[2] = 3.14 *
94  ((uint16_t)(custom_controller_ref.encoder6_data[0] << 8) |
95  (uint16_t)custom_controller_ref.encoder6_data[1]) /
96  18000.0;
97  custom_controller_data.joystick_l_y_data = ((uint16_t)(custom_controller_ref.joystick_l_x_data[0] << 8) |
98  (uint16_t)custom_controller_ref.joystick_l_x_data[1]);
99  custom_controller_data.joystick_l_x_data = ((uint16_t)(custom_controller_ref.joystick_l_y_data[0] << 8) |
100  (uint16_t)custom_controller_ref.joystick_l_y_data[1]);
101  custom_controller_data.joystick_r_y_data = ((uint16_t)(custom_controller_ref.joystick_r_x_data[0] << 8) |
102  (uint16_t)custom_controller_ref.joystick_r_x_data[1]);
103  custom_controller_data.joystick_r_x_data = ((uint16_t)(custom_controller_ref.joystick_r_y_data[0] << 8) |
104  (uint16_t)custom_controller_ref.joystick_r_y_data[1]);
105  custom_controller_data.button_data[0] = custom_controller_ref.button1_data;
106  custom_controller_data.button_data[1] = custom_controller_ref.button2_data;
107  custom_controller_data.button_data[2] = custom_controller_ref.button3_data;
108  custom_controller_data.button_data[3] = custom_controller_ref.button4_data;
109  custom_controller_cmd_pub_.publish(custom_controller_data);
110  break;
111  }
112  case rm_vt::ROBOT_COMMAND_CMD:
113  {
114  rm_vt::KeyboardMouseData keyboard_mouse_ref;
115  rm_msgs::VTKeyboardMouseData keyboard_mouse_data;
116  memcpy(&keyboard_mouse_ref, rx_data + 7, sizeof(rm_vt::KeyboardMouseData));
117  keyboard_mouse_data.mouse_x = keyboard_mouse_ref.mouse_x;
118  keyboard_mouse_data.mouse_y = keyboard_mouse_ref.mouse_y;
119  keyboard_mouse_data.mouse_z = keyboard_mouse_ref.mouse_z;
120  keyboard_mouse_data.left_button_down = keyboard_mouse_ref.left_button_down;
121  keyboard_mouse_data.right_button_down = keyboard_mouse_ref.right_button_down;
122  keyboard_mouse_data.key_w = keyboard_mouse_ref.key_w;
123  keyboard_mouse_data.key_s = keyboard_mouse_ref.key_s;
124  keyboard_mouse_data.key_a = keyboard_mouse_ref.key_a;
125  keyboard_mouse_data.key_d = keyboard_mouse_ref.key_d;
126  keyboard_mouse_data.key_shift = keyboard_mouse_ref.key_shift;
127  keyboard_mouse_data.key_ctrl = keyboard_mouse_ref.key_ctrl;
128  keyboard_mouse_data.key_q = keyboard_mouse_ref.key_q;
129  keyboard_mouse_data.key_e = keyboard_mouse_ref.key_e;
130  keyboard_mouse_data.key_r = keyboard_mouse_ref.key_r;
131  keyboard_mouse_data.key_f = keyboard_mouse_ref.key_f;
132  keyboard_mouse_data.key_g = keyboard_mouse_ref.key_g;
133  keyboard_mouse_data.key_z = keyboard_mouse_ref.key_z;
134  keyboard_mouse_data.key_x = keyboard_mouse_ref.key_x;
135  keyboard_mouse_data.key_c = keyboard_mouse_ref.key_c;
136  keyboard_mouse_data.key_v = keyboard_mouse_ref.key_v;
137  keyboard_mouse_data.key_b = keyboard_mouse_ref.key_b;
138  vt_keyboard_mouse_pub_.publish(keyboard_mouse_data);
139  break;
140  }
141  default:
142  ROS_WARN("Referee command ID %d not found.", cmd_id);
143  break;
144  }
145  base_.video_transmission_is_online_ = true;
146  last_get_data_time_ = ros::Time::now();
147  return frame_len;
148  }
149  }
150  return -1;
151 }
152 
153 int VideoTransmission::control_data_unpack(uint8_t* rx_data)
154 {
155  int frame_len = 21;
156  if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1)
157  {
158  rm_vt::ControlData control_ref;
159  rm_msgs::VTReceiverControlData control_data;
160  memcpy(&control_ref, rx_data + 2, sizeof(rm_vt::ControlData));
161  control_data.joystick_r_x = (control_ref.joystick_r_x - 1024.0) / 660.0;
162  control_data.joystick_r_y = (control_ref.joystick_r_y - 1024.0) / 660.0;
163  control_data.joystick_l_y = (control_ref.joystick_l_y - 1024.0) / 660.0;
164  control_data.joystick_l_x = (control_ref.joystick_l_x - 1024.0) / 660.0;
165  control_data.mode_switch = control_ref.mode_switch;
166  control_data.pause_button = control_ref.pause_button;
167  control_data.custom_button_l = control_ref.custom_button_l;
168  control_data.custom_button_r = control_ref.custom_button_r;
169  control_data.wheel = (control_ref.wheel - 1024.0) / 660.0;
170  control_data.trigger = control_ref.trigger;
171  control_data.mouse_x = control_ref.mouse_x;
172  control_data.mouse_y = control_ref.mouse_y;
173  control_data.mouse_wheel = control_ref.mouse_wheel;
174  control_data.mouse_left_down = control_ref.mouse_left_down;
175  control_data.mouse_right_down = control_ref.mouse_right_down;
176  control_data.mouse_mid_down = control_ref.mouse_mid_down;
177  control_data.key_w = control_ref.key_w;
178  control_data.key_s = control_ref.key_s;
179  control_data.key_a = control_ref.key_a;
180  control_data.key_d = control_ref.key_d;
181  control_data.key_shift = control_ref.key_shift;
182  control_data.key_ctrl = control_ref.key_ctrl;
183  control_data.key_q = control_ref.key_q;
184  control_data.key_e = control_ref.key_e;
185  control_data.key_r = control_ref.key_r;
186  control_data.key_f = control_ref.key_f;
187  control_data.key_g = control_ref.key_g;
188  control_data.key_z = control_ref.key_z;
189  control_data.key_x = control_ref.key_x;
190  control_data.key_c = control_ref.key_c;
191  control_data.key_v = control_ref.key_v;
192  control_data.key_b = control_ref.key_b;
193  vt_receiver_control_pub_.publish(control_data);
194 
195  base_.video_transmission_is_online_ = true;
196  last_get_data_time_ = ros::Time::now();
197  return frame_len;
198  }
199  return -1;
200 }
201 } // namespace rm_vt
serial::Serial::read
std::string read(size_t size=1)
rm_vt::VideoTransmission::clearRxBuffer
void clearRxBuffer()
Definition: video_transmission.h:25
rm_vt::Base::serial_
serial::Serial serial_
Definition: data.h:30
rm_vt::CUSTOM_CONTROLLER_CMD
@ CUSTOM_CONTROLLER_CMD
Definition: protocol.h:12
rm_vt::VideoTransmission::unpack_buffer_
uint8_t unpack_buffer_[256]
Definition: video_transmission.h:43
rm_vt::VideoTransmission::unpack
int unpack(uint8_t *rx_data)
Definition: video_transmission.cpp:48
rm_vt::VideoTransmission::last_get_data_time_
ros::Time last_get_data_time_
Definition: video_transmission.h:40
rm_vt
Definition: data.h:25
rm_vt::VideoTransmission::k_unpack_buffer_length_
const int k_unpack_buffer_length_
Definition: video_transmission.h:42
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rm_vt::VideoTransmission::custom_controller_cmd_pub_
ros::Publisher custom_controller_cmd_pub_
Definition: video_transmission.h:31
video_transmission.h
rm_vt::VideoTransmission::rx_len_
int rx_len_
Definition: video_transmission.h:35
rm_vt::VideoTransmission::control_data_unpack
int control_data_unpack(uint8_t *rx_data)
Definition: video_transmission.cpp:153
rm_vt::VideoTransmission::k_header_length_
const int k_header_length_
Definition: video_transmission.h:41
rm_vt::VideoTransmission::read
void read()
Definition: video_transmission.cpp:8
rm_vt::VideoTransmission::k_cmd_id_length_
const int k_cmd_id_length_
Definition: video_transmission.h:41
rm_vt::VideoTransmission::vt_receiver_control_pub_
ros::Publisher vt_receiver_control_pub_
Definition: video_transmission.h:31
ROS_WARN
#define ROS_WARN(...)
rm_vt::Base::verifyCRC8CheckSum
uint32_t verifyCRC8CheckSum(unsigned char *pch_message, unsigned int dw_length)
Definition: data.h:63
rm_vt::Base::verifyCRC16CheckSum
uint32_t verifyCRC16CheckSum(uint8_t *pch_message, uint32_t dw_length)
Definition: data.h:95
rm_vt::VideoTransmission::rx_buffer_
std::vector< uint8_t > rx_buffer_
Definition: video_transmission.h:34
rm_vt::ROBOT_COMMAND_CMD
@ ROBOT_COMMAND_CMD
Definition: protocol.h:13
rm_vt::VideoTransmission::base_
Base base_
Definition: video_transmission.h:33
rm_vt::VideoTransmission::vt_keyboard_mouse_pub_
ros::Publisher vt_keyboard_mouse_pub_
Definition: video_transmission.h:31
rm_vt::VideoTransmission::k_tail_length_
const int k_tail_length_
Definition: video_transmission.h:41
rm_vt::VideoTransmission::k_frame_length_
const int k_frame_length_
Definition: video_transmission.h:41
serial::Serial::available
size_t available()
ROS_INFO
#define ROS_INFO(...)
ros::Duration
rm_vt::Base::video_transmission_is_online_
bool video_transmission_is_online_
Definition: data.h:31
ros::Time::now
static Time now()

rm_vt
Author(s):
autogenerated on Tue May 6 2025 02:23:53