diff --git a/CMakeLists.txt b/CMakeLists.txt index de6b475..09fcdd9 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,11 +81,10 @@ add_library(${PROJECT_NAME} ######################## ## protocol v2 support src/protocol_v2/agilex_msg_parser_v2.c - src/protocol_v2/parser_impl/protocol_v2_parser.cpp + src/protocol_v2/protocol_v2_parser.cpp ######################## ## legacy protocol v1 support (transparent to user) src/protocol_v1/agilex_msg_parser_v1.c - src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp ) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index 7b7a16f..7b73d66 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -331,7 +331,7 @@ typedef struct { // V1-only messages MotionCommandMessageV1 v1_motion_command_msg; ValueSetCommandMessageV1 v1_value_set_command_msg; - ActuatorStateMessageV1 v1_actuator_stage_msg; + ActuatorStateMessageV1 v1_actuator_state_msg; } body; } AgxMessage; diff --git a/src/protocol_v1/agilex_msg_parser_v1.h b/include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h similarity index 100% rename from src/protocol_v1/agilex_msg_parser_v1.h rename to include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h diff --git a/src/protocol_v1/agilex_protocol_v1.h b/include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h similarity index 100% rename from src/protocol_v1/agilex_protocol_v1.h rename to include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h diff --git a/include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp new file mode 100644 index 0000000..4e64e0b --- /dev/null +++ b/include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp @@ -0,0 +1,66 @@ +/* + * scout_protocol_v1_parser.hpp + * + * Created on: Jul 08, 2021 22:30 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP +#define SCOUT_PROTOCOL_V1_PARSER_HPP + +#include "ugv_sdk/details/interface/parser_interface.hpp" + +#include "ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h" +#include "ugv_sdk/details/protocol_v1/robot_limits.hpp" + +namespace westonrobot { +template +class DiffDriveModelV1Parser : public ParserInterface { + public: + bool DecodeMessage(const struct can_frame *rx_frame, + AgxMessage *msg) override { + return DecodeCanFrameV1(rx_frame, msg); + } + + void EncodeMessage(const AgxMessage *msg, + struct can_frame *tx_frame) override { + AgxMessage msg_v1 = *msg; + if (msg->type == AgxMsgMotionCommand) { + float linear = msg->body.motion_command_msg.linear_velocity; + float angular = msg->body.motion_command_msg.angular_velocity; + if (linear > ScoutV2Limits::max_linear_velocity) + linear = ScoutV2Limits::max_linear_velocity; + else if (linear < ScoutV2Limits::min_linear_velocity) + linear = ScoutV2Limits::min_linear_velocity; + if (angular > ScoutV2Limits::max_angular_velocity) + angular = ScoutV2Limits::max_angular_velocity; + else if (angular < ScoutV2Limits::min_angular_velocity) + angular = ScoutV2Limits::min_angular_velocity; + msg_v1.body.motion_command_msg.linear_velocity = + linear / ScoutV2Limits::max_linear_velocity * 100.0; + msg_v1.body.motion_command_msg.angular_velocity = static_cast( + angular / ScoutV2Limits::max_angular_velocity * 100.0); + } + EncodeCanFrameV1(&msg_v1, tx_frame); + } + + uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override { + return CalcCanFrameChecksumV1(id, data, dlc); + } + + // UART support + bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {} + + void EncodeMessage(const AgxMessage *msg, uint8_t *buf, + uint8_t *len) override {} + + uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {} +}; + +using ScoutProtocolV1Parser = DiffDriveModelV1Parser; +using ScoutMiniProtocolV1Parser = DiffDriveModelV1Parser; +} // namespace westonrobot + +#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */ diff --git a/include/ugv_sdk/details/protocol_v1/hunter_protocol_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/hunter_protocol_v1_parser.hpp deleted file mode 100644 index 5737d1f..0000000 --- a/include/ugv_sdk/details/protocol_v1/hunter_protocol_v1_parser.hpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * hunter_protocol_v1_parser.hpp - * - * Created on: Jul 09, 2021 22:20 - * Description: - * - * Copyright (c) 2021 Ruixiang Du (rdu) - */ - -#ifndef HUNTER_PROTOCOL_V1_PARSER_HPP -#define HUNTER_PROTOCOL_V1_PARSER_HPP - -#include "ugv_sdk/details/interface/parser_interface.hpp" - -namespace westonrobot { -class HunterProtocolV1Parser : public ParserInterface { - public: - bool DecodeMessage(const struct can_frame *rx_frame, - AgxMessage *msg) override; - void EncodeMessage(const AgxMessage *msg, - struct can_frame *tx_frame) override; - uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override; -}; -} // namespace westonrobot - -#endif /* HUNTER_PROTOCOL_V1_PARSER_HPP */ diff --git a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp new file mode 100644 index 0000000..4b9cbfa --- /dev/null +++ b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp @@ -0,0 +1,29 @@ +/* + * robot_limits.hpp + * + * Created on: Jul 12, 2021 14:01 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef ROBOT_LIMITS_HPP +#define ROBOT_LIMITS_HPP + +namespace westonrobot { +struct ScoutV2Limits { + static constexpr double max_linear_velocity = 1.5; // 1.5 m/s + static constexpr double min_linear_velocity = -max_linear_velocity; + static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s + static constexpr double min_angular_velocity = -max_angular_velocity; +}; + +struct ScoutMiniLimits { + static constexpr double max_linear_velocity = 3.0; // 3.0 m/s + static constexpr double min_linear_velocity = -max_linear_velocity; + static constexpr double max_angular_velocity = 2.5235; // 2.5235 rad/s + static constexpr double min_angular_velocity = -max_angular_velocity; +}; +} // namespace westonrobot + +#endif /* ROBOT_LIMITS_HPP */ diff --git a/include/ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp deleted file mode 100644 index 73b8e6b..0000000 --- a/include/ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp +++ /dev/null @@ -1,32 +0,0 @@ -/* - * scout_protocol_v1_parser.hpp - * - * Created on: Jul 08, 2021 22:30 - * Description: - * - * Copyright (c) 2021 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP -#define SCOUT_PROTOCOL_V1_PARSER_HPP - -#include "ugv_sdk/details/interface/parser_interface.hpp" - -namespace westonrobot { -class ScoutProtocolV1Parser : public ParserInterface { - public: - bool DecodeMessage(const struct can_frame *rx_frame, - AgxMessage *msg) override; - void EncodeMessage(const AgxMessage *msg, - struct can_frame *tx_frame) override; - uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override; - - // UART support - bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override; - void EncodeMessage(const AgxMessage *msg, uint8_t *buf, - uint8_t *len) override; - uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override; -}; -} // namespace westonrobot - -#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */ diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index 7ad7150..2539159 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -84,10 +84,6 @@ class ScoutBase : public AgilexBase, public ScoutInterface { state.light_state = status_msg.body.light_state_msg; break; } - // case AgxMsgRcState: { - // state.rc_state = status_msg.body.rc_state_msg; - // break; - // } case AgxMsgActuatorHSState: { // std::cout << "actuator hs feedback received" << std::endl; state @@ -102,6 +98,10 @@ class ScoutBase : public AgilexBase, public ScoutInterface { status_msg.body.actuator_ls_state_msg; break; } + // case AgxMsgRcState: { + // state.rc_state = status_msg.body.rc_state_msg; + // break; + // } /* sensor feedback */ // case AgxMsgOdometry: { // // std::cout << "Odometer msg feedback received" << std::endl; @@ -114,7 +114,7 @@ class ScoutBase : public AgilexBase, public ScoutInterface { }; } // namespace westonrobot -#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp" +#include "ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp" #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" namespace westonrobot { diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c index 257cadd..f8d2d16 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.c +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -7,12 +7,130 @@ * Copyright (c) 2021 Ruixiang Du (rdu) */ -#include "agilex_protocol_v1.h" -#include "protocol_v1/agilex_msg_parser_v1.h" +#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h" +#include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h" #include "stdio.h" #include "string.h" -bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {} -void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {} -uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {} +bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { + switch (rx_frame->can_id) { + case CAN_MSG_SYSTEM_STATE_ID: { + msg->type = AgxMsgSystemState; + // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; + // memcpy(msg->body.system_state_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTION_STATE_ID: { + msg->type = AgxMsgMotionState; + // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; + // memcpy(msg->body.motion_state_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_LIGHT_STATE_ID: { + msg->type = AgxMsgLightState; + // msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + // memcpy(msg->body.light_state_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR1_STATE_ID: { + msg->type = AgxMsgActuatorStateV1; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + // msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR1_ID; + // memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR2_STATE_ID: { + msg->type = AgxMsgActuatorStateV1; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; + // msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR2_ID; + // memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR3_STATE_ID: { + msg->type = AgxMsgActuatorStateV1; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; + // msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR3_ID; + // memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_ACTUATOR4_STATE_ID: { + msg->type = AgxMsgActuatorStateV1; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; + // msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR4_ID; + // memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, + // rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + default: + break; + } + + return true; +} + +void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { + switch (msg->type) { + case AgxMsgMotionCommandV1: { + static uint8_t count = 0; + tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID; + tx_frame->can_dlc = 8; + tx_frame->data[0] = CTRL_MODE_CMD_CAN; + tx_frame->data[1] = ERROR_CLR_NONE; + tx_frame->data[2] = + (int8_t)(msg->body.motion_command_msg.linear_velocity * 100); + tx_frame->data[3] = + (int8_t)(msg->body.motion_command_msg.angular_velocity * 100); + tx_frame->data[4] = 0; + tx_frame->data[5] = 0; + tx_frame->data[6] = count++; + tx_frame->data[7] = CalcCanFrameChecksumV1( + tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); + break; + } + case AgxMsgValueSetCommandV1: { + static uint8_t count = 0; + tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID; + tx_frame->can_dlc = 8; + LightCommandFrame frame; + if (msg->body.light_command_msg.enable_cmd_ctrl) { + frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL; + frame.front_mode = msg->body.light_command_msg.front_light.mode; + 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; + } else { + frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL; + frame.front_mode = 0; + frame.front_custom = 0; + frame.rear_mode = 0; + frame.rear_custom = 0; + } + frame.reserved0 = 0; + frame.count = count++; + memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); + tx_frame->data[7] = CalcCanFrameChecksumV1( + tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); + break; + } + case AgxMsgLightCommand: { + break; + } + default: + break; + } +} + +uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) { + uint8_t checksum = 0x00; + checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; + for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; + return checksum; +} diff --git a/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp b/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp deleted file mode 100644 index 981a5a5..0000000 --- a/src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * scout_protocol_v1_parser.cpp - * - * Created on: Jul 08, 2021 22:43 - * Description: - * - * Copyright (c) 2021 Ruixiang Du (rdu) - */ - -#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp" - -namespace westonrobot { -// CAN support -bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame, - AgxMessage *msg) { - // ScoutMessage scout_msg; - // if ScoutMessage found, convert to AgxMessage - // if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) { - // switch (scout_msg.type) { - // case ScoutMotionStatusMsg: { - // break; - // } - // case ScoutLightStatusMsg: { - // break; - // } - // case ScoutSystemStatusMsg: { - // break; - // } - // case ScoutMotorDriverStatusMsg: { - // break; - // } - // } - // } - return false; -} - -void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, - struct can_frame *tx_frame) { - // ScoutMessage scout_msg; - // convert to ScoutMessage, then encode to can frame - // switch (msg->type) { - // case AgxMsgMotionCommand: { - // // scout_msg.type = ScoutMotionControlMsg; - // // scout_msg.body.motion_control_msg.data.cmd.control_mode = - // // CTRL_MODE_COMMANDED; - // // scout_msg.body.motion_control_msg.data.cmd.control_mode = - // // CTRL_MODE_CMD_CAN; - // // scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag = - // 0x00; - - // /* - // std::lock_guard guard(motion_cmd_mutex_); - // current_motion_cmd_.linear_velocity = static_cast( - // linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0); - // current_motion_cmd_.angular_velocity = static_cast( - // angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0); - // */ - // // scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = - // // current_motion_cmd_.linear_velocity; - // // scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = - // // current_motion_cmd_.angular_velocity; - // // scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0; - // // scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0; - // // scout_msg.body.motion_control_msg.data.cmd.count = count; - // // scout_msg.body.motion_control_msg.data.cmd.checksum = - // // CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, - // // scout_msg.body.motion_control_msg.data.raw, - // 8); - // break; - // } - // case AgxMsgLightCommand: { - // scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable = - // LIGHT_ENABLE_CTRL; - - // // scout_msg.body.light_control_msg.data.cmd.front_mode = - // // static_cast(current_light_cmd_.front_mode); - // // scout_msg.body.light_control_msg.data.cmd.front_custom = - // // current_light_cmd_.front_custom_value; - // // scout_msg.body.light_control_msg.data.cmd.rear_mode = - // // static_cast(current_light_cmd_.rear_mode); - // // scout_msg.body.light_control_msg.data.cmd.rear_custom = - // // current_light_cmd_.rear_custom_value; - // // scout_msg.body.light_control_msg.data.cmd.reserved0 = 0; - // // scout_msg.body.light_control_msg.data.cmd.count = count; - - // scout_msg.body.light_control_msg.data.cmd.checksum = - // CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, - // scout_msg.body.light_control_msg.data.raw, 8); - - // break; - // } - // } -} - -uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data, - uint8_t dlc) { - uint8_t checksum = 0x00; - checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; - for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; - return checksum; -} - -// UART support -bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc, - AgxMessage *msg) {} - -void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf, - uint8_t *len) {} - -uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {} -} // namespace westonrobot diff --git a/src/protocol_v2/parser_impl/protocol_v2_parser.cpp b/src/protocol_v2/protocol_v2_parser.cpp similarity index 100% rename from src/protocol_v2/parser_impl/protocol_v2_parser.cpp rename to src/protocol_v2/protocol_v2_parser.cpp