From f7224bf56f044fbf0df48e60dc572499a51bbae2 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 13 Jul 2021 18:03:50 +0800 Subject: [PATCH] added get protocol version api --- demo/scout_demo/scout_robot_demo.cpp | 37 ++++++++++++++----- .../details/interface/parser_interface.hpp | 12 +++++- .../details/interface/robot_interface.hpp | 7 ++-- .../details/interface/scout_interface.hpp | 2 +- .../protocol_v1/agilex_msg_parser_v1.h | 2 +- .../protocol_v1/protocol_v1_parser.hpp | 6 +-- .../protocol_v2/protocol_v2_parser.hpp | 4 +- .../details/robot_base/agilex_base.hpp | 19 +++++----- include/ugv_sdk/mobile_robot/scout_robot.hpp | 4 +- src/mobile_robot/scout_robot.cpp | 10 +++-- src/protocol_v1/agilex_msg_parser_v1.c | 18 ++++++--- src/protocol_v2/agilex_msg_parser_v2.c | 24 ++++++------ src/protocol_v2/agilex_msg_parser_v2.h | 2 +- src/protocol_v2/protocol_v2_parser.cpp | 4 +- 14 files changed, 96 insertions(+), 55 deletions(-) diff --git a/demo/scout_demo/scout_robot_demo.cpp b/demo/scout_demo/scout_robot_demo.cpp index ed4bbf2..250a162 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -34,9 +34,11 @@ int main(int argc, char **argv) { std::unique_ptr scout; if (protocol_version == "v1") { - scout = std::unique_ptr(new ScoutRobot(ProtocolType::AGX_V1)); + scout = + std::unique_ptr(new ScoutRobot(ProtocolVersion::AGX_V1)); } else if (protocol_version == "v2") { - scout = std::unique_ptr(new ScoutRobot(ProtocolType::AGX_V2)); + scout = + std::unique_ptr(new ScoutRobot(ProtocolVersion::AGX_V2)); } else { std::cout << "Error: invalid protocol version string" << std::endl; return -1; @@ -46,7 +48,13 @@ int main(int argc, char **argv) { std::cout << "Failed to create robot object" << std::endl; scout->Connect(device_name); - scout->EnableCommandedMode(); + + if (scout->GetProtocolVersion() == ProtocolVersion::AGX_V2) { + scout->EnableCommandedMode(); + std::cout << "Protocol version 2" << std::endl; + } else { + std::cout << "Protocol version 1" << std::endl; + } // light control std::cout << "Light: const off" << std::endl; @@ -67,10 +75,10 @@ int main(int argc, char **argv) { int count = 0; while (true) { // motion control - if (count < 100) { - std::cout << "Motor: 0.2, 0" << std::endl; - scout->SetMotionCommand(0.2, 0.0); - } + // if (count < 100) { + // std::cout << "Motor: 0.2, 0" << std::endl; + // scout->SetMotionCommand(0.2, 0.0); + // } // get robot state auto state = scout->GetRobotState(); @@ -80,12 +88,23 @@ int main(int argc, char **argv) { << static_cast(state.system_state.control_mode) << " , vehicle state: " << static_cast(state.system_state.vehicle_state) - << std::endl; - std::cout << "battery voltage: " << state.system_state.battery_voltage + << " , error code: " << std::hex << state.system_state.error_code + << ", battery voltage: " << state.system_state.battery_voltage << std::endl; std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " << state.motion_state.angular_velocity << std::endl; + + if (scout->GetProtocolVersion() == ProtocolVersion::AGX_V1) { + for (int i = 0; i < 4; ++i) { + printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n", + state.actuator_state[i].motor_id, + state.actuator_state[i].current, state.actuator_state[i].rpm, + state.actuator_state[i].driver_temp, + state.actuator_state[i].motor_temp); + } + } else { + } std::cout << "-------------------------------" << std::endl; usleep(20000); diff --git a/include/ugv_sdk/details/interface/parser_interface.hpp b/include/ugv_sdk/details/interface/parser_interface.hpp index 3d63008..38e7ee8 100644 --- a/include/ugv_sdk/details/interface/parser_interface.hpp +++ b/include/ugv_sdk/details/interface/parser_interface.hpp @@ -26,11 +26,17 @@ struct can_frame { #include "ugv_sdk/details/interface/agilex_message.h" -struct ParserInterface { +enum class ProtocolVersion { AGX_V1, AGX_V2 }; + +template +class ParserInterface { + public: + virtual ~ParserInterface() = default; + // CAN support virtual bool DecodeMessage(const struct can_frame *rx_frame, AgxMessage *msg) = 0; - virtual void EncodeMessage(const AgxMessage *msg, + virtual bool EncodeMessage(const AgxMessage *msg, struct can_frame *tx_frame) = 0; virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) = 0; @@ -47,6 +53,8 @@ struct ParserInterface { // throw exception return 0; }; + + ProtocolVersion GetProtocolVersion() { return VersionNumber; } }; #endif /* PASER_INTERFACE_HPP */ diff --git a/include/ugv_sdk/details/interface/robot_interface.hpp b/include/ugv_sdk/details/interface/robot_interface.hpp index 8d71221..75e38f9 100644 --- a/include/ugv_sdk/details/interface/robot_interface.hpp +++ b/include/ugv_sdk/details/interface/robot_interface.hpp @@ -13,10 +13,9 @@ #include #include "ugv_sdk/details/interface/agilex_message.h" +#include "ugv_sdk/details/interface/parser_interface.hpp" namespace westonrobot { -enum class ProtocolType { AGX_V1, AGX_V2 }; - class RobotInterface { public: ~RobotInterface() = default; @@ -33,12 +32,14 @@ class RobotInterface { }; virtual void ResetRobotState() = 0; + virtual ProtocolVersion GetProtocolVersion() = 0; + protected: /****** functions not available/valid to all robots ******/ // functions to be implemented by class AgilexBase virtual void SetMotionMode(uint8_t mode){}; - // any specific robot will use a specialized version of the two functions + // any specific robot will use a specialized version of the two functions virtual void SendMotionCommand(double linear_vel, double angular_vel, double lateral_velocity, double steering_angle){ diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp index 61c491c..ac003c1 100644 --- a/include/ugv_sdk/details/interface/scout_interface.hpp +++ b/include/ugv_sdk/details/interface/scout_interface.hpp @@ -29,7 +29,7 @@ struct ScoutState { }; struct ScoutInterface { - ~ScoutInterface() = default; + virtual ~ScoutInterface() = default; virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0; virtual void SetLightCommand(LightMode f_mode, uint8_t f_value, diff --git a/include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h b/include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h index 5f560e5..affcc23 100644 --- a/include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h +++ b/include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h @@ -31,7 +31,7 @@ struct can_frame { #include "ugv_sdk/details/interface/agilex_message.h" bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg); -void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame); +bool EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame); uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc); #ifdef __cplusplus diff --git a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp index c21783f..d8a1eb9 100644 --- a/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -17,14 +17,14 @@ namespace westonrobot { template -class ProtocolV1Parser : public ParserInterface { +class ProtocolV1Parser : public ParserInterface { public: bool DecodeMessage(const struct can_frame *rx_frame, AgxMessage *msg) override { return DecodeCanFrameV1(rx_frame, msg); } - void EncodeMessage(const AgxMessage *msg, + bool EncodeMessage(const AgxMessage *msg, struct can_frame *tx_frame) override { AgxMessage msg_v1 = *msg; if (msg->type == AgxMsgMotionCommandV1) { @@ -44,7 +44,7 @@ class ProtocolV1Parser : public ParserInterface { msg_v1.body.v1_motion_command_msg.angular = angular / RobotLimitsType::max_angular * 100.0; } - EncodeCanFrameV1(&msg_v1, tx_frame); + return EncodeCanFrameV1(&msg_v1, tx_frame); } uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override { diff --git a/include/ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp b/include/ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp index 0a18909..78a7c5c 100644 --- a/include/ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp +++ b/include/ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp @@ -13,11 +13,11 @@ #include "ugv_sdk/details/interface/parser_interface.hpp" namespace westonrobot { -class ProtocolV2Parser : public ParserInterface { +class ProtocolV2Parser : public ParserInterface { public: bool DecodeMessage(const struct can_frame *rx_frame, AgxMessage *msg) override; - void EncodeMessage(const AgxMessage *msg, + bool EncodeMessage(const AgxMessage *msg, struct can_frame *tx_frame) override; uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override; }; diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index c463ed2..ae04b4a 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -46,8 +46,7 @@ class AgilexBase : public RobotInterface { // encode msg to can frame and send to bus can_frame frame; - parser_.EncodeMessage(&msg, &frame); - can_->SendFrame(frame); + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); } // must be called at a frequency >= 50Hz @@ -64,8 +63,7 @@ class AgilexBase : public RobotInterface { // send to can bus can_frame frame; - parser_.EncodeMessage(&msg, &frame); - can_->SendFrame(frame); + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); } } @@ -83,8 +81,7 @@ class AgilexBase : public RobotInterface { // send to can bus can_frame frame; - parser_.EncodeMessage(&msg, &frame); - can_->SendFrame(frame); + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); } } @@ -97,8 +94,7 @@ class AgilexBase : public RobotInterface { // send to can bus can_frame frame; - parser_.EncodeMessage(&msg, &frame); - can_->SendFrame(frame); + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); } } @@ -111,11 +107,14 @@ class AgilexBase : public RobotInterface { // send to can bus can_frame frame; - parser_.EncodeMessage(&msg, &frame); - can_->SendFrame(frame); + if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame); } } + ProtocolVersion GetProtocolVersion() override { + return parser_.GetProtocolVersion(); + } + protected: ParserType parser_; std::mutex state_mutex_; diff --git a/include/ugv_sdk/mobile_robot/scout_robot.hpp b/include/ugv_sdk/mobile_robot/scout_robot.hpp index f757850..f6b2bf4 100644 --- a/include/ugv_sdk/mobile_robot/scout_robot.hpp +++ b/include/ugv_sdk/mobile_robot/scout_robot.hpp @@ -18,7 +18,7 @@ namespace westonrobot { class ScoutRobot : public RobotInterface, public ScoutInterface { public: - ScoutRobot(ProtocolType protocol = ProtocolType::AGX_V2); + ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2); ~ScoutRobot(); void Connect(std::string can_name) override; @@ -33,6 +33,8 @@ class ScoutRobot : public RobotInterface, public ScoutInterface { void ResetRobotState() override; + ProtocolVersion GetProtocolVersion() override; + // get robot state ScoutState GetRobotState() override; diff --git a/src/mobile_robot/scout_robot.cpp b/src/mobile_robot/scout_robot.cpp index 9207eb4..7f3e0d7 100644 --- a/src/mobile_robot/scout_robot.cpp +++ b/src/mobile_robot/scout_robot.cpp @@ -11,10 +11,10 @@ #include "ugv_sdk/details/robot_base/scout_base.hpp" namespace westonrobot { -ScoutRobot::ScoutRobot(ProtocolType protocol) { - if (protocol == ProtocolType::AGX_V1) { +ScoutRobot::ScoutRobot(ProtocolVersion protocol) { + if (protocol == ProtocolVersion::AGX_V1) { robot_ = new ScoutBaseV1(); - } else if (protocol == ProtocolType::AGX_V2) { + } else if (protocol == ProtocolVersion::AGX_V2) { robot_ = new ScoutBaseV2(); } } @@ -33,6 +33,10 @@ void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) { void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); } +ProtocolVersion ScoutRobot::GetProtocolVersion() { + return robot_->GetProtocolVersion(); +} + void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) { auto scout = dynamic_cast(robot_); scout->SetMotionCommand(linear_vel, angular_vel); diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c index d7c5152..79cd375 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.c +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -18,6 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { // if checksum not correct if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data, rx_frame->can_dlc)) { + printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id); return false; } @@ -36,13 +37,16 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { case CAN_MSG_MOTION_STATE_ID: { msg->type = AgxMsgMotionState; msg->body.motion_state_msg.linear_velocity = - ((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) / + ((int16_t)((((uint16_t)rx_frame->data[0]) << 8) | + (uint16_t)rx_frame->data[1])) / 1000.0f; msg->body.motion_state_msg.angular_velocity = - ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) / + ((int16_t)((((uint16_t)rx_frame->data[2]) << 8) | + (uint16_t)rx_frame->data[3])) / 1000.0f; msg->body.motion_state_msg.lateral_velocity = - ((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]) / + ((int16_t)((((uint16_t)rx_frame->data[4]) << 8) | + (uint16_t)rx_frame->data[5])) / 1000.0f; break; } @@ -89,7 +93,8 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { return true; } -void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { +bool EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { + bool ret = true; switch (msg->type) { case AgxMsgMotionCommandV1: { static uint8_t count = 0; @@ -155,9 +160,12 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); break; } - default: + default: { + ret = false; break; + } } + return ret; } uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) { diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index aad8838..2c9acc5 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -50,8 +50,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { msg->body.light_command_msg.front_light.custom_value = frame->front_custom; msg->body.light_command_msg.rear_light.mode = frame->rear_mode; - msg->body.light_command_msg.rear_light.custom_value = - frame->rear_custom; + msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom; break; } case CAN_MSG_BRAKING_COMMAND_ID: { @@ -104,8 +103,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { msg->body.light_command_msg.front_light.custom_value = frame->front_custom; msg->body.light_command_msg.rear_light.mode = frame->rear_mode; - msg->body.light_command_msg.rear_light.custom_value = - frame->rear_custom; + msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom; break; } case CAN_MSG_RC_STATE_ID: { @@ -192,10 +190,9 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { msg->body.actuator_ls_state_msg.driver_state = frame->driver_state; break; } - case CAN_MSG_CURRENT_CTRL_MODE: - { - msg->type=AgxMsgMotionModeState; - MotionModeStateFrame *frame = (MotionModeStateFrame*)(rx_frame->data); + case CAN_MSG_CURRENT_CTRL_MODE: { + msg->type = AgxMsgMotionModeState; + MotionModeStateFrame *frame = (MotionModeStateFrame *)(rx_frame->data); msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode; msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing; @@ -309,7 +306,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) { return true; } -void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { +bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { + bool ret = true; switch (msg->type) { /***************** command frame *****************/ case AgxMsgMotionCommand: { @@ -346,8 +344,7 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { frame.front_custom = msg->body.light_command_msg.front_light.custom_value; frame.rear_mode = msg->body.light_command_msg.rear_light.mode; - frame.rear_custom = - msg->body.light_command_msg.rear_light.custom_value; + frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value; } else { frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL; frame.front_mode = 0; @@ -590,9 +587,12 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); break; } - default: + default: { + ret = false; break; + } } + return ret; } uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc) { diff --git a/src/protocol_v2/agilex_msg_parser_v2.h b/src/protocol_v2/agilex_msg_parser_v2.h index bec53c8..aa3593a 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.h +++ b/src/protocol_v2/agilex_msg_parser_v2.h @@ -31,7 +31,7 @@ struct can_frame { #include "ugv_sdk/details/interface/agilex_message.h" bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg); -void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame); +bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame); uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc); #ifdef __cplusplus diff --git a/src/protocol_v2/protocol_v2_parser.cpp b/src/protocol_v2/protocol_v2_parser.cpp index a7a5375..0e36a6f 100644 --- a/src/protocol_v2/protocol_v2_parser.cpp +++ b/src/protocol_v2/protocol_v2_parser.cpp @@ -16,9 +16,9 @@ bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame, return DecodeCanFrameV2(rx_frame, msg); } -void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg, +bool ProtocolV2Parser::EncodeMessage(const AgxMessage *msg, struct can_frame *tx_frame) { - EncodeCanFrameV2(msg, tx_frame); + return EncodeCanFrameV2(msg, tx_frame); } uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data,