我是靠谱客的博主 知性棉花糖，这篇文章主要介绍松灵机器人Scout代码分析 --- scout_rosscout_base_node.cpp1. 通讯连接并设置回调函数2 读取数据 及 更新状态数据3. 发送指令4 灯光控制5 通讯协议解析，现在分享给大家，希望可以做个参考。
公司采购了一款室外的四轮差速底盘，正在看通讯的代码。
代码的github: https://github.com/westonrobot/scout_ros
scout_base_node.cpp
1. 调用 ScoutBase.h 的 connetc() : 设置通讯方式（serial or can ）并 进行连接
2. scout_base/src/scout_messenger.h 的 SetupSubscription() : 发布odom, scout_status topic, 设置订阅 /cmd_vel, /scout_light_control 的回调函数, 频率为20hz
3. 发布/scout_status，机器人状态
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43int main(int argc, char **argv) { // setup ROS node ros::init(argc, argv, "scout_odom"); ros::NodeHandle node("scout_robot"), private_node("~"); // instantiate a robot ScoutBase robot; ScoutROSMessenger messenger(&robot, node); std::string scout_can_port; private_node.param<std::string>("port_name", scout_can_port, std::string("can0")); private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom")); private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_footprint")); private_node.param<bool>("simulated_robot", messenger.simulated_robot_, false); // connect to scout and setup ROS subscription robot.Connect(scout_can_port); messenger.SetupSubscription(); // publish robot state at 20Hz while listening to twist commands ros::Rate rate_20hz(20); // 20Hz while (true) { messenger.PublishStateToROS(); ros::spinOnce(); rate_20hz.sleep(); } return 0; } // resigeter callback function void ScoutROSMessenger::SetupSubscription() { // odometry publisher odom_publisher_ = nh_.advertise<nav_msgs::Odometry>(odom_frame_, 50); status_publisher_ = nh_.advertise<scout_msgs::ScoutStatus>("/scout_status", 10); // cmd subscriber motion_cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel” light_cmd_subscriber_ = nh_.subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); }
1. 通讯连接并设置回调函数
scout_base/src/scout_sdk/src/scout_base/src/scout_base.cpp
1.1 自定义的消息类型
每次消息到来之后，首先转换消息类型，转成一个 ScoutStatusMessage 这个结构体，这个结构体每次只更新一项，由msg_type设置。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112typedef struct { ScoutStatusMsgType msg_type; // only one of the following fields is updated, as specified by msg_type MotionStatusMessage motion_status_msg; LightStatusMessage light_status_msg; SystemStatusMessage system_status_msg; MotorDriverStatusMessage motor_driver_status_msg; } ScoutStatusMessage; // For convenience to access status/control message typedef enum { ScoutStatusNone = 0x00, ScoutMotionStatusMsg = 0x01, ScoutLightStatusMsg = 0x02, ScoutSystemStatusMsg = 0x03, ScoutMotorDriverStatusMsg = 0x04 } ScoutStatusMsgType; typedef struct { union { struct { struct { uint8_t high_byte; uint8_t low_byte; } linear_velocity; struct { uint8_t high_byte; uint8_t low_byte; } angular_velocity; uint8_t reserved0; uint8_t reserved1; uint8_t count; uint8_t checksum; } status; uint8_t raw[8]; } data; } MotionStatusMessage; typedef struct { union { struct { uint8_t light_ctrl_enable; uint8_t front_light_mode; uint8_t front_light_custom; uint8_t rear_light_mode; uint8_t rear_light_custom; uint8_t reserved0; uint8_t count; uint8_t checksum; } status; uint8_t raw[8]; } data; } LightStatusMessage; typedef struct { union { struct { uint8_t base_state; uint8_t control_mode; struct { uint8_t high_byte; uint8_t low_byte; } battery_voltage; struct { uint8_t high_byte; uint8_t low_byte; } fault_code; uint8_t count; uint8_t checksum; } status; uint8_t raw[8]; } data; } SystemStatusMessage; // Motor Driver Feedback typedef struct { uint8_t motor_id; union { struct { struct { uint8_t high_byte; uint8_t low_byte; } current; struct { uint8_t high_byte; uint8_t low_byte; } rpm; int8_t temperature; uint8_t reserved0; uint8_t count; uint8_t checksum; } status; uint8_t raw[8]; } data; } MotorDriverStatusMessage;
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38// choose serial or Can communication void ScoutBase::Connect(std::string dev_name, int32_t baud_rate) { if (baud_rate == 0) { ConfigureCANBus(dev_name); } else { ConfigureSerial(dev_name, baud_rate); if (!serial_connected_) std::cerr << "ERROR: Failed to connect to serial port" << std::endl; } } void ScoutBase::ConfigureCANBus(const std::string &can_if_name) { can_if_ = std::make_shared<ASyncCAN>(can_if_name); can_if_->set_receive_callback(std::bind(&ScoutBase::ParseCANFrame, this, std::placeholders::_1)); can_connected_ = true; } void ScoutBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate) { serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate); serial_if_->open(); if (serial_if_->is_open()) serial_connected_ = true; serial_if_->set_receive_callback(std::bind(&ScoutBase::ParseUARTBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); }
2 读取数据 及 更新状态数据
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89void ScoutBase::ParseCANFrame(can_frame *rx_frame) { // validate checksum, discard frame if fails if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc)) { std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl; return; } // otherwise, update robot state with new frame ScoutStatusMessage status_msg; DecodeScoutStatusMsgFromCAN(rx_frame, &status_msg); NewStatusMsgReceivedCallback(status_msg); } void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) { // std::cout << "bytes received from serial: " << bytes_received << std::endl; ScoutStatusMessage status_msg; for (int i = 0; i < bytes_received; ++i) { if (DecodeScoutStatusMsgFromUART(buf[i], &status_msg)) NewStatusMsgReceivedCallback(status_msg); } } void ScoutBase::NewStatusMsgReceivedCallback(const ScoutStatusMessage &msg) { // std::cout << "new status msg received" << std::endl; std::lock_guard<std::mutex> guard(scout_state_mutex_); UpdateScoutState(msg, scout_state_); } void ScoutBase::UpdateScoutState(const ScoutStatusMessage &status_msg, ScoutState &state) { switch (status_msg.msg_type) { case ScoutMotionStatusMsg: { // std::cout << "motion control feedback received" << std::endl; const MotionStatusMessage &msg = status_msg.motion_status_msg; state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0; state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0; break; } case ScoutLightStatusMsg: { // std::cout << "light control feedback received" << std::endl; const LightStatusMessage &msg = status_msg.light_status_msg; if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) state.light_control_enabled = false; else state.light_control_enabled = true; state.front_light_state.mode = msg.data.status.front_light_mode; state.front_light_state.custom_value = msg.data.status.front_light_custom; state.rear_light_state.mode = msg.data.status.rear_light_mode; state.rear_light_state.custom_value = msg.data.status.rear_light_custom; break; } case ScoutSystemStatusMsg: { // std::cout << "system status feedback received" << std::endl; const SystemStatusMessage &msg = status_msg.system_status_msg; state.control_mode = msg.data.status.control_mode; state.base_state = msg.data.status.base_state; state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0; state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8); break; } case ScoutMotorDriverStatusMsg: { // std::cout << "motor 1 driver feedback received" << std::endl; const MotorDriverStatusMessage &msg = status_msg.motor_driver_status_msg; for (int i = 0; i < 4; ++i) { state.motor_states[status_msg.motor_driver_status_msg.motor_id].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0; state.motor_states[status_msg.motor_driver_status_msg.motor_id].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8); state.motor_states[status_msg.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature; } break; } } }
3. 发送指令
3.1 设置 线速度，角速度。
订阅 /cmd_vel ，之后将收到的线速度，角速度 除以 最大值 再乘以 100，得到 线速度，角速度的百分比。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38motion_cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); // scout_->SetMotionCommand(msg->linear.x, msg->angular.z); void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) { if (!simulated_robot_) { scout_->SetMotionCommand(msg->linear.x, msg->angular.z); } else { std::lock_guard<std::mutex> guard(twist_mutex_); current_twist_ = *msg.get(); } // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); } void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag) { // make sure cmd thread is started before attempting to send commands if (!cmd_thread_started_) StartCmdThread(); if (linear_vel < ScoutMotionCmd::min_linear_velocity) linear_vel = ScoutMotionCmd::min_linear_velocity; if (linear_vel > ScoutMotionCmd::max_linear_velocity) linear_vel = ScoutMotionCmd::max_linear_velocity; if (angular_vel < ScoutMotionCmd::min_angular_velocity) angular_vel = ScoutMotionCmd::min_angular_velocity; if (angular_vel > ScoutMotionCmd::max_angular_velocity) angular_vel = ScoutMotionCmd::max_angular_velocity; std::lock_guard<std::mutex> guard(motion_cmd_mutex_); current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0); current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); current_motion_cmd_.fault_clear_flag = fault_clr_flag; }
3.2 内置的消息类型
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18// Motion Control typedef struct { union { struct { uint8_t control_mode; uint8_t fault_clear_flag; int8_t linear_velocity_cmd; int8_t angular_velocity_cmd; uint8_t reserved0; uint8_t reserved1; uint8_t count; uint8_t checksum; } cmd; uint8_t raw[8]; } data; } MotionControlMessage;
3.3 定时向底层发送数据(100hz)
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71// 开启 速度 cmd 线程 ScoutBase::ControlLoop void ScoutBase::StartCmdThread() { current_motion_cmd_.linear_velocity = 0; current_motion_cmd_.angular_velocity = 0; current_motion_cmd_.fault_clear_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT; // cmd thread runs at 100Hz (10ms) by default ; cmd_thread_period_ms_ = 10 cmd_thread_ = std::thread(std::bind(&ScoutBase::ControlLoop, this, cmd_thread_period_ms_)); cmd_thread_started_ = true; } void ScoutBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; uint8_t cmd_count = 0; uint8_t light_cmd_count = 0; while (true) { ctrl_sw.tic(); // motion control message SendMotionCmd(cmd_count++); // check if there is request for light control if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); ctrl_sw.sleep_until_ms(period_ms); // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl; } } void ScoutBase::SendMotionCmd(uint8_t count) { // motion control message MotionControlMessage m_msg; if (can_connected_) m_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; else if (serial_connected_) m_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; motion_cmd_mutex_.lock(); m_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag); m_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity; m_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity; motion_cmd_mutex_.unlock(); m_msg.data.cmd.reserved0 = 0; m_msg.data.cmd.reserved1 = 0; m_msg.data.cmd.count = count; if (can_connected_) m_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, m_msg.data.raw, 8); // serial_connected_: checksum will be calculated later when packed into a complete serial frame if (can_connected_) { // send to can bus can_frame m_frame; EncodeScoutMotionControlMsgToCAN(&m_msg, &m_frame); can_if_->send_frame(m_frame); } else { // send to serial port EncodeMotionControlMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); } }
4 灯光控制
4.1 ROS 下的消息类型
复制代码
1
2
3
4
5
6
7
8
9
10uint8 LIGHT_CONST_OFF = 0 uint8 LIGHT_CONST_ON = 1 uint8 LIGHT_BREATH = 2 uint8 LIGHT_CUSTOM = 3 bool enable_cmd_light_control uint8 front_mode uint8 front_custom_value uint8 rear_mode uint8 rear_custom_value
4.2 自定义消息类型
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39struct ScoutLightCmd { enum class LightMode { CONST_OFF = 0x00, CONST_ON = 0x01, BREATH = 0x02, CUSTOM = 0x03 }; ScoutLightCmd() = default; ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), rear_mode(r_mode), rear_custom_value(r_value) {} LightMode front_mode; uint8_t front_custom_value; LightMode rear_mode; uint8_t rear_custom_value; }; // Light Control typedef struct { union { struct { uint8_t light_ctrl_enable; uint8_t front_light_mode; uint8_t front_light_custom; uint8_t rear_light_mode; uint8_t rear_light_custom; uint8_t reserved0; uint8_t count; uint8_t checksum; } cmd; uint8_t raw[8]; } data; } LightControlMessage;
4.3 设置灯光
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85light_cmd_subscriber_ = nh_.subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); // scout_->SetLightCommand(cmd); void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) { if (!simulated_robot_) { if (msg->enable_cmd_light_control) { ScoutLightCmd cmd; switch (msg->front_mode) { case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF; break; } case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; break; } case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { cmd.front_mode = ScoutLightCmd::LightMode::BREATH; break; } case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.front_custom_value = msg->front_custom_value; break; } } switch (msg->rear_mode) { case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF; break; } case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON; break; } case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { cmd.rear_mode = ScoutLightCmd::LightMode::BREATH; break; } case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.rear_custom_value = msg->rear_custom_value; break; } } scout_->SetLightCommand(cmd); } else { scout_->DisableLightCmdControl(); } } else { std::cout << "simulated robot received light control cmd" << std::endl; } } void ScoutBase::SetLightCommand(ScoutLightCmd cmd) { if (!cmd_thread_started_) StartCmdThread(); std::lock_guard<std::mutex> guard(light_cmd_mutex_); current_light_cmd_ = cmd; light_ctrl_enabled_ = true; light_ctrl_requested_ = true; }
4.4 发送灯光控制指令
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49void ScoutBase::SendLightCmd(uint8_t count) { LightControlMessage l_msg; light_cmd_mutex_.lock(); if (light_ctrl_enabled_) { l_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; l_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode); l_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value; l_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode); l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value; } else { l_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL; l_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF; l_msg.data.cmd.front_light_custom = 0; l_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF; l_msg.data.cmd.rear_light_custom = 0; } light_ctrl_requested_ = false; light_cmd_mutex_.unlock(); l_msg.data.cmd.reserved0 = 0; l_msg.data.cmd.count = count; if (can_connected_) l_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.data.raw, 8); // serial_connected_: checksum will be calculated later when packed into a complete serial frame if (can_connected_) { // send to can bus can_frame l_frame; EncodeScoutLightControlMsgToCAN(&l_msg, &l_frame); can_if_->send_frame(l_frame); } else { // send to serial port EncodeLightControlMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); } }
5 通讯协议解析
5.1 串口通讯
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230typedef union { ScoutStatusMessage status_msg; ScoutControlMessage control_msg; } ScoutDecodedMessage; // c为读取的字节 bool DecodeScoutStatusMsgFromUART(uint8_t c, ScoutStatusMessage *msg) { static ScoutDecodedMessage decoded_msg; bool result = ParseChar(c, &decoded_msg); if (result) *msg = decoded_msg.status_msg; return result; } bool ParseChar(uint8_t c, ScoutDecodedMessage *msg) { static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1; bool new_frame_parsed = false; switch (decode_state) { case WAIT_FOR_SOF1: { if (c == FRAME_SOF1) { frame_id = FRAME_NONE_ID; frame_type = 0; frame_len = 0; frame_cnt = 0; frame_checksum = 0; internal_checksum = 0; payload_data_pos = 0; memset(payload_buffer, 0, PAYLOAD_BUFFER_SIZE); decode_state = WAIT_FOR_SOF2; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "found sof1" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "found sof1n"); #endif } break; } case WAIT_FOR_SOF2: { if (c == FRAME_SOF2) { decode_state = WAIT_FOR_FRAME_LEN; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "found sof2" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "found sof2n"); #endif } else { decode_state = WAIT_FOR_SOF1; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "failed to find sof2" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "failed to find sof2n"); #endif } break; } case WAIT_FOR_FRAME_LEN: { frame_len = c; decode_state = WAIT_FOR_FRAME_TYPE; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkRTTPrintf(0, "frame len: %dn", frame_len); #endif break; } case WAIT_FOR_FRAME_TYPE: { switch (c) { case FRAME_TYPE_CONTROL: { frame_type = FRAME_TYPE_CONTROL; decode_state = WAIT_FOR_FRAME_ID; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "control type frame received" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "control type frame receivedn"); #endif break; } case FRAME_TYPE_STATUS: { frame_type = FRAME_TYPE_STATUS; decode_state = WAIT_FOR_FRAME_ID; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "status type frame received" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "status type frame receivedn"); #endif break; } default: { #ifdef PRINT_CPP_DEBUG_INFO std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUSn"); #endif decode_state = WAIT_FOR_SOF1; } } break; } case WAIT_FOR_FRAME_ID: { switch (c) { case UART_FRAME_SYSTEM_STATUS_ID: case UART_FRAME_MOTION_STATUS_ID: case UART_FRAME_MOTOR1_DRIVER_STATUS_ID: case UART_FRAME_MOTOR2_DRIVER_STATUS_ID: case UART_FRAME_MOTOR3_DRIVER_STATUS_ID: case UART_FRAME_MOTOR4_DRIVER_STATUS_ID: case UART_FRAME_LIGHT_STATUS_ID: { frame_id = c; decode_state = WAIT_FOR_PAYLOAD; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkRTTPrintf(0, "frame id: %dn", frame_id); #endif break; } default: { #ifdef PRINT_CPP_DEBUG_INFO std::cerr << "ERROR: Unknown frame id" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "ERROR: Unknown frame idn"); #endif decode_state = WAIT_FOR_SOF1; } } break; } case WAIT_FOR_PAYLOAD: { payload_buffer[payload_data_pos++] = c; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkRTTPrintf(0, "1 byte added: %dn", c); #endif if (payload_data_pos == (frame_len - FRAME_FIXED_FIELD_LEN)) decode_state = WAIT_FOR_FRAME_COUNT; break; } case WAIT_FOR_FRAME_COUNT: { frame_cnt = c; decode_state = WAIT_FOR_CHECKSUM; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkRTTPrintf(0, "frame count: %dn", frame_cnt); #endif break; } case WAIT_FOR_CHECKSUM: { frame_checksum = c; internal_checksum = CalcBufferedFrameChecksum(); new_frame_parsed = true; decode_state = WAIT_FOR_SOF1; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl; std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkRTTPrintf(0, "--- frame checksum: : %dn", frame_checksum); JLinkRTTPrintf(0, "--- internal frame checksum: : %dn", internal_checksum); #endif break; } default: break; } if (new_frame_parsed) { if (frame_checksum == internal_checksum) { #ifdef PRINT_CPP_DEBUG_INFO std::cout << "checksum correct" << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "checksum correctn"); #endif if (frame_type == FRAME_TYPE_STATUS) ConstructStatusMessage(&(msg->status_msg)); else if (frame_type == FRAME_TYPE_CONTROL) ConstructControlMessage(&(msg->control_msg)); ++frame_parsed; } else { ++frame_with_wrong_checksum; #ifdef PRINT_CPP_DEBUG_INFO std::cout << "checksum is NOT correct" << std::endl; std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl; std::cout << "payload: "; for (int i = 0; i < payload_data_pos; ++i) std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " "; std::cout << std::endl; std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl; std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl; #elif (defined(PRINT_JLINK_DEBUG_INFO)) JLinkWriteString(0, "checksum is NOT correctn"); #endif } } return new_frame_parsed; }
5.2 can通讯
最后
以上就是知性棉花糖最近收集整理的关于松灵机器人Scout代码分析 --- scout_rosscout_base_node.cpp1. 通讯连接并设置回调函数2 读取数据 及 更新状态数据3. 发送指令4 灯光控制5 通讯协议解析的全部内容，更多相关松灵机器人Scout代码分析内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复