mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

agilex_base: fixed memory issue related to can_ interface

Browse Source
This commit is contained in:
rdu
2022-03-16 14:52:40 +08:00
parent 3815cc4438
commit 9b7fc33d38
1 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
include/ugv_sdk/details/robot_base/agilex_base.hpp
View File

@@ -54,14 +54,16 @@ class AgilexBase : public RobotCommonInterface {
msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN; msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN;
// encode msg to can frame and send to bus // encode msg to can frame and send to bus
if (can_ != nullptr && can_->IsOpened()) {
can_frame frame; can_frame frame;
if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
} }
}
// must be called at a frequency >= 50Hz // must be called at a frequency >= 50Hz
void SendMotionCommand(double linear_vel, double angular_vel, void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_vel, double steering_angle) { double lateral_vel, double steering_angle) {
if (can_->IsOpened()) { if (can_ != nullptr && can_->IsOpened()) {
// motion control message // motion control message
AgxMessage msg; AgxMessage msg;
if (parser_.GetParserProtocolVersion() == ProtocolVersion::AGX_V1) { if (parser_.GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
@@ -95,7 +97,7 @@ class AgilexBase : public RobotCommonInterface {
// one-shot light command // one-shot light command
void SendLightCommand(AgxLightMode front_mode, uint8_t front_custom_value, void SendLightCommand(AgxLightMode front_mode, uint8_t front_custom_value,
AgxLightMode rear_mode, uint8_t rear_custom_value) { AgxLightMode rear_mode, uint8_t rear_custom_value) {
if (can_->IsOpened()) { if (can_ != nullptr && can_->IsOpened()) {
AgxMessage msg; AgxMessage msg;
msg.type = AgxMsgLightCommand; msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.enable_cmd_ctrl = true; msg.body.light_command_msg.enable_cmd_ctrl = true;
@@ -111,7 +113,7 @@ class AgilexBase : public RobotCommonInterface {
} }
void DisableLightControl() { void DisableLightControl() {
if (can_->IsOpened()) { if (can_ != nullptr && can_->IsOpened()) {
AgxMessage msg; AgxMessage msg;
msg.type = AgxMsgLightCommand; msg.type = AgxMsgLightCommand;
@@ -125,7 +127,7 @@ class AgilexBase : public RobotCommonInterface {
// motion mode change // motion mode change
void SetMotionMode(uint8_t mode) { void SetMotionMode(uint8_t mode) {
if (can_->IsOpened()) { if (can_ != nullptr && can_->IsOpened()) {
AgxMessage msg; AgxMessage msg;
msg.type = AgxMsgSetMotionModeCommand; msg.type = AgxMsgSetMotionModeCommand;
msg.body.motion_mode_msg.motion_mode = mode; msg.body.motion_mode_msg.motion_mode = mode;
@@ -181,7 +183,7 @@ class AgilexBase : public RobotCommonInterface {
} }
void DisconnectPort() { void DisconnectPort() {
if (can_->IsOpened()) can_->Close(); if (can_ != nullptr && can_->IsOpened()) can_->Close();
} }
void SetBrakeMode(AgxBrakeMode mode) { void SetBrakeMode(AgxBrakeMode mode) {
@@ -191,9 +193,11 @@ class AgilexBase : public RobotCommonInterface {
msg.body.brake_mode_config_msg.mode = mode; msg.body.brake_mode_config_msg.mode = mode;
// encode msg to can frame and send to bus // encode msg to can frame and send to bus
if (can_ != nullptr && can_->IsOpened()) {
can_frame frame; can_frame frame;
if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
} }
}
void ParseCANFrame(can_frame *rx_frame) { void ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg; AgxMessage status_msg;