松灵机器人Scout代码分析 --- scout_ros
2024-04-14 03:45:56
公司采购了一款室外的四轮差速底盘，正在看通讯的代码。
代码的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，机器人状态
int main(int argc, char **argv)
{// setup ROS noderos::init(argc, argv, "scout_odom");ros::NodeHandle node("scout_robot"), private_node("~");// instantiate a robotScoutBase 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 subscriptionrobot.Connect(scout_can_port);messenger.SetupSubscription();// publish robot state at 20Hz while listening to twist commandsros::Rate rate_20hz(20); // 20Hzwhile (true){messenger.PublishStateToROS();ros::spinOnce();rate_20hz.sleep();}return 0;
}// resigeter callback function
void ScoutROSMessenger::SetupSubscription()
{// odometry publisherodom_publisher_ = nh_.advertise<nav_msgs::Odometry>(odom_frame_, 50);status_publisher_ = nh_.advertise<scout_msgs::ScoutStatus>("/scout_status", 10);// cmd subscribermotion_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设置。
typedef struct
{ScoutStatusMsgType msg_type;// only one of the following fields is updated, as specified by msg_typeMotionStatusMessage 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;// 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 读取数据 及 更新状态数据
void ScoutBase::ParseCANFrame(can_frame *rx_frame)
{// validate checksum, discard frame if failsif (!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 frameScoutStatusMessage 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;elsestate.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，得到 线速度，角速度的百分比。
motion_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 commandsif (!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 内置的消息类型
// 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)
// 开启 速度 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_ = 10cmd_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 messageSendMotionCmd(cmd_count++);// check if there is request for light controlif (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 messageMotionControlMessage 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 frameif (can_connected_){// send to can buscan_frame m_frame;EncodeScoutMotionControlMsgToCAN(&m_msg, &m_frame);can_if_->send_frame(m_frame);}else{// send to serial portEncodeMotionControlMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);}
}4 灯光控制
4.1 ROS 下的消息类型
uint8 LIGHT_CONST_OFF = 0
uint8 LIGHT_CONST_ON = 1
uint8 LIGHT_BREATH = 2
uint8 LIGHT_CUSTOM = 3bool enable_cmd_light_control
uint8 front_mode
uint8 front_custom_value
uint8 rear_mode
uint8 rear_custom_value4.2 自定义消息类型
struct 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 设置灯光
light_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 发送灯光控制指令
void 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 frameif (can_connected_){// send to can buscan_frame l_frame;EncodeScoutLightControlMsgToCAN(&l_msg, &l_frame);can_if_->send_frame(l_frame);}else{// send to serial portEncodeLightControlMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);}}5 通讯协议解析
5.1 串口通讯
typedef 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_INFOstd::cout << "found sof1" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "found sof1\n");
#endif}break;}case WAIT_FOR_SOF2:{if (c == FRAME_SOF2){decode_state = WAIT_FOR_FRAME_LEN;
#ifdef PRINT_CPP_DEBUG_INFOstd::cout << "found sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "found sof2\n");
#endif}else{decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFOstd::cout << "failed to find sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "failed to find sof2\n");
#endif}break;}case WAIT_FOR_FRAME_LEN:{frame_len = c;decode_state = WAIT_FOR_FRAME_TYPE;
#ifdef PRINT_CPP_DEBUG_INFOstd::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkRTTPrintf(0, "frame len: %d\n", frame_len);
#endifbreak;}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_INFOstd::cout << "control type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "control type frame received\n");
#endifbreak;}case FRAME_TYPE_STATUS:{frame_type = FRAME_TYPE_STATUS;decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFOstd::cout << "status type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "status type frame received\n");
#endifbreak;}default:{
#ifdef PRINT_CPP_DEBUG_INFOstd::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_STATUS\n");
#endifdecode_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_INFOstd::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkRTTPrintf(0, "frame id: %d\n", frame_id);
#endifbreak;}default:{
#ifdef PRINT_CPP_DEBUG_INFOstd::cerr << "ERROR: Unknown frame id" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "ERROR: Unknown frame id\n");
#endifdecode_state = WAIT_FOR_SOF1;}}break;}case WAIT_FOR_PAYLOAD:{payload_buffer[payload_data_pos++] = c;
#ifdef PRINT_CPP_DEBUG_INFOstd::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: %d\n", c);
#endifif (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_INFOstd::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt);
#endifbreak;}case WAIT_FOR_CHECKSUM:{frame_checksum = c;internal_checksum = CalcBufferedFrameChecksum();new_frame_parsed = true;decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFOstd::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: : %d\n", frame_checksum);JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum);
#endifbreak;}default:break;}if (new_frame_parsed){if (frame_checksum == internal_checksum){
#ifdef PRINT_CPP_DEBUG_INFOstd::cout << "checksum correct" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))JLinkWriteString(0, "checksum correct\n");
#endifif (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_INFOstd::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 correct\n");
#endif}}return new_frame_parsed;
}5.2 can通讯
松灵机器人Scout代码分析 --- scout_ros相关推荐
- 松灵机器人二次开发之一
松灵机器人二次开发--第一天 缘由: 最近学院有一个机器人二次开发的项目.我和朋友被导师推荐了过去,结果去了一头雾水:松灵机器人,linux,导航探测~~对于两个纯软件方面的菜鸟来说毫无头绪.但是没办 ...
- 松灵机器人二次开发总结
松灵机器人二次开发最终总结 前言: 因为最近写完了系统的专利,因此想在这里给故事划上一个句号.前面说到自动检测和报表形成是比较难的操作,其中一个是本系统的核心操作功能,一个是系统的难实现功能.都是本系 ...
- 三、松灵课堂 | SCOUT的仿真使用
仿真环境的介绍 Gazebo Gazebo是一款3D动态模拟器,能够在复杂的室内和室外环境中准确有效地模拟机器人群.与游戏引擎提供高保真度的视觉模拟类似,Gazebo提供高保真度的物理模拟,其提供一整 ...
- 松灵学院 | Scout mini 仿真指南
本文适合有 ROS1 基础与了解 urdf语法 和 gazebo 的朋友 SCOUT MINI 是一款全地形高速Mini UGV,具有四轮差速驱动.独立悬挂.原地差速自转等特点,得益于自主研发的轻量级 ...
- SCOUT MINI Pro松灵机器人j建图定点步骤
一.slam建图实践: (1)启动激光雷达,发布base_link-><laser_link>的坐标变换 source devel/setup.bash roslaunch scou ...
- 操作ＲＯＳ松灵机器人步骤及遇到的问题
小车型号:scout-v2.0 https://github.com/agilexrobotics/scout_ros(所用包的下载地址) 或者 https://hub.fastgit.org/agi ...
- 全球与中国自动停车机器人市场深度研究分析报告
[报告篇幅]:112 [报告图表数]:159 [报告出版时间]:2022年3月 报告摘要 2021年全球自动停车机器人市场销售额达到了 亿美元,预计2028年将达到 亿美元,年复合增长率(CAGR)为 ...
- 【工程记录】基于松灵Scout mini底盘实现小车自主探索建图
一.硬件配置 松灵scout mini底盘 速腾robense 16线激光雷达 瑞芬imu 因特尔nuc 二.工程技术栈 IMU驱动包(原创) 松灵底盘包(改写,里程计融合IMU角度信息) ROS N ...
- vins中imu融合_VINS-Mono代码分析与总结(最终版)
VINS-Mono代码分析总结 参考文献 前言 ??视觉与IMU融合的分类: 松耦合和紧耦合:按照是否把图像的Feature加入到状态向量区分,换句话说就是松耦合是在视觉和IMU各自求出的位姿的基础上 ...
- VINS-Mono代码分析与总结(完整版)
VINS-Mono代码分析总结 参考文献 1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, ...
最新文章
- ORM 框架中SQLALCHEMY一点点个人总结
- 本人使用Intelij idea问题及解决汇总
- leetcode231
- mysql 获取当前日期及格式化
- JAVA笔记11__File类/File类作业/字节输出流、输入流/字符输出流、输入流/文件复制/转换流...
- 【Java】《Java编程的逻辑》第6章 异常 笔记+感悟分享
- pytorch新手需要注意的隐晦操作Tensor,max,gather
- oracle数据库rman备份与还原
- eXosip注册函数与使用说明
- 因为文件格式或文件扩展名无效。请确定文件未损坏，并且文件扩展名与文件的格式匹配
- 计算机桌面文字重影,电脑桌面字有重影怎么办
- vue中json数据格式化
- 车秘android版本最新版本,车秘下载2021安卓最新版_手机app官方版免费安装下载_豌豆荚...
- 一键式统计6.0通透
- Productivity Power Tools工具
- 怎样把gis锯齿边_在arcgis中如何消除锯齿状边缘
- 简述计算机安全的三种类型
- Lua 错误之 attempt to index a function value
- pytorch TorchScript 简介
- elasticsearch8.2集群部署