diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..701763e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "googletest"] + path = test/googletest + url = https://github.com/google/googletest.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 09fcdd9..60684c3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,8 +8,8 @@ if(CCACHE_PROGRAM) endif() ## Project Options -option(BUILD_TESTS ON) -option(STATIC_CHECK OFF) +option(BUILD_TESTING "Build tests" OFF) +option(STATIC_CHECK "Run static check" OFF) # set(CMAKE_BUILD_TYPE Release) # set(CMAKE_BUILD_TYPE Debug) @@ -56,9 +56,19 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) endif() if(BUILD_WITHOUT_ROS) + # check whether to build tests + if(PROJECT_NAME STREQUAL CMAKE_PROJECT_NAME AND BUILD_TESTING) + enable_testing() + include(GoogleTest) + set(BUILD_TESTS ON) + message(STATUS "Tests will be built") + else() + set(BUILD_TESTS OFF) + message(STATUS "Tests will not be built") + endif() + ## Use GNUInstallDirs to install libraries into correct locations on all platforms. include(GNUInstallDirs) - message(STATUS "Project will be installed to ${CMAKE_INSTALL_PREFIX} with 'make install'") ## Put all binary files into /bin and libraries into /lib set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}) @@ -94,34 +104,19 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ PRIVATE src) -# Build apps +# Build demo if(BUILD_WITHOUT_ROS) - # Check wether to build monitor tool - set(BUILD_MONITOR ON) - # Disable monitor if ncurses library is not found - set(CURSES_NEED_NCURSES TRUE) - find_package(Curses QUIET) - if(BUILD_MONITOR AND NOT CURSES_FOUND) - set(BUILD_MONITOR OFF) - message(STATUS "Monitor app will not be built due to missing ncurses library, try: sudo apt install libncurses5-dev") - endif() - - # add app source directory add_subdirectory(demo) endif() # Build tests -# if(PROJECT_NAME STREQUAL CMAKE_PROJECT_NAME AND BUILD_TESTS) -# message(STATUS "Tests will be built") -# enable_testing() -# include(GoogleTest) -# add_subdirectory(tests) -# else() -# message(STATUS "Tests will not be built") -# endif() +if(BUILD_TESTS) + add_subdirectory(test) +endif() ## Installation configuration if(BUILD_WITHOUT_ROS) # BUILD_WITHOUT_ROS + message(STATUS "Project will be installed to ${CMAKE_INSTALL_PREFIX} with 'make install'") # Offer the user the choice of overriding the installation directories set(INSTALL_LIBDIR ${CMAKE_INSTALL_LIBDIR} CACHE PATH "Installation directory for libraries") diff --git a/include/ugv_sdk/details/interface/agilex_message.h b/include/ugv_sdk/details/interface/agilex_message.h index 7b73d66..f62af5e 100644 --- a/include/ugv_sdk/details/interface/agilex_message.h +++ b/include/ugv_sdk/details/interface/agilex_message.h @@ -42,7 +42,7 @@ typedef struct { typedef struct { uint8_t motion_mode; -} MotionModeMessage; +} MotionModeCommandMessage; // V1-only messages typedef struct { @@ -56,6 +56,8 @@ typedef struct { bool set_neutral; } ValueSetCommandMessageV1; +typedef ValueSetCommandMessageV1 ValueSetStateMessageV1; + /**************** Feedback messages *****************/ #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) @@ -128,6 +130,7 @@ typedef struct { // V1-only messages typedef struct { + uint8_t motor_id; float current; int16_t rpm; float temperature; @@ -263,7 +266,7 @@ typedef enum { AgxMsgMotionCommand, AgxMsgLightCommand, AgxMsgBrakingCommand, - AgxMsgSetMotionMode, + AgxMsgSetMotionModeCommand, // state feedback AgxMsgSystemState, AgxMsgMotionState, @@ -292,6 +295,7 @@ typedef enum { // V1-only messages AgxMsgMotionCommandV1, AgxMsgValueSetCommandV1, + AgxMsgValueSetStateV1, AgxMsgActuatorStateV1 } MsgType; @@ -302,7 +306,7 @@ typedef struct { MotionCommandMessage motion_command_msg; LightCommandMessage light_command_msg; BrakingCommandMessage braking_command_msg; - MotionModeMessage motion_mode_msg; + MotionModeCommandMessage motion_mode_msg; // state feedback SystemStateMessage system_state_msg; MotionStateMessage motion_state_msg; @@ -331,6 +335,7 @@ typedef struct { // V1-only messages MotionCommandMessageV1 v1_motion_command_msg; ValueSetCommandMessageV1 v1_value_set_command_msg; + ValueSetStateMessageV1 v1_value_set_state_msg; ActuatorStateMessageV1 v1_actuator_state_msg; } body; } AgxMessage; diff --git a/include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp similarity index 53% rename from include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp rename to include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp index 4e64e0b..c21783f 100644 --- a/include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp +++ b/include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp @@ -17,7 +17,7 @@ namespace westonrobot { template -class DiffDriveModelV1Parser : public ParserInterface { +class ProtocolV1Parser : public ParserInterface { public: bool DecodeMessage(const struct can_frame *rx_frame, AgxMessage *msg) override { @@ -27,21 +27,22 @@ class DiffDriveModelV1Parser : public ParserInterface { 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); + if (msg->type == AgxMsgMotionCommandV1) { + float linear = msg->body.v1_motion_command_msg.linear; + float angular = msg->body.v1_motion_command_msg.angular; + if (linear > RobotLimitsType::max_linear) + linear = RobotLimitsType::max_linear; + else if (linear < RobotLimitsType::min_linear) + linear = RobotLimitsType::min_linear; + if (angular > RobotLimitsType::max_angular) + angular = RobotLimitsType::max_angular; + else if (angular < RobotLimitsType::min_angular) + angular = RobotLimitsType::min_angular; + + msg_v1.body.v1_motion_command_msg.linear = + linear / RobotLimitsType::max_linear * 100.0; + msg_v1.body.v1_motion_command_msg.angular = + angular / RobotLimitsType::max_angular * 100.0; } EncodeCanFrameV1(&msg_v1, tx_frame); } @@ -59,8 +60,11 @@ class DiffDriveModelV1Parser : public ParserInterface { uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {} }; -using ScoutProtocolV1Parser = DiffDriveModelV1Parser; -using ScoutMiniProtocolV1Parser = DiffDriveModelV1Parser; +using ScoutProtocolV1Parser = ProtocolV1Parser; +using ScoutMiniProtocolV1Parser = ProtocolV1Parser; + +using HunterProtocolV1Parser = ProtocolV1Parser; + } // namespace westonrobot #endif /* SCOUT_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 index 4b9cbfa..f3f5a6d 100644 --- a/include/ugv_sdk/details/protocol_v1/robot_limits.hpp +++ b/include/ugv_sdk/details/protocol_v1/robot_limits.hpp @@ -12,17 +12,24 @@ 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; + static constexpr double max_linear = 1.5; // m/s + static constexpr double min_linear = -max_linear; + static constexpr double max_angular = 0.5235; // rad/s + static constexpr double min_angular = -max_angular; }; 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; + static constexpr double max_linear = 3.0; // m/s + static constexpr double min_linear = -max_linear; + static constexpr double max_angular = 2.5235; // rad/s + static constexpr double min_angular = -max_angular; +}; + +struct HunterV1Limits { + static constexpr double max_linear = 1.5; // m/s + static constexpr double min_linear = -max_linear; + static constexpr double max_angular = 25.5; // degree + static constexpr double min_angular = -max_angular; }; } // namespace westonrobot diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index 68bc2e3..c463ed2 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -106,7 +106,7 @@ class AgilexBase : public RobotInterface { void SetMotionMode(uint8_t mode) { if (can_connected_) { AgxMessage msg; - msg.type = AgxMsgSetMotionMode; + msg.type = AgxMsgSetMotionModeCommand; msg.body.motion_mode_msg.motion_mode = mode; // send to can bus diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp index 2539159..f1fa90e 100644 --- a/include/ugv_sdk/details/robot_base/scout_base.hpp +++ b/include/ugv_sdk/details/robot_base/scout_base.hpp @@ -114,7 +114,7 @@ class ScoutBase : public AgilexBase, public ScoutInterface { }; } // namespace westonrobot -#include "ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp" +#include "ugv_sdk/details/protocol_v1/protocol_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 f8d2d16..ca24c45 100644 --- a/src/protocol_v1/agilex_msg_parser_v1.c +++ b/src/protocol_v1/agilex_msg_parser_v1.c @@ -7,26 +7,40 @@ * Copyright (c) 2021 Ruixiang Du (rdu) */ -#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h" #include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h" +#include "protocol_v1/agilex_protocol_v1.h" + #include "stdio.h" #include "string.h" 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)) { + return false; + } + 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)); + msg->body.system_state_msg.vehicle_state = rx_frame->data[0]; + msg->body.system_state_msg.control_mode = rx_frame->data[1]; + msg->body.system_state_msg.battery_voltage = + ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) / + 10.0f; + msg->body.system_state_msg.error_code = + ((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]); 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)); + msg->body.motion_state_msg.linear_velocity = + ((((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]) / + 1000.0f; break; } case CAN_MSG_LIGHT_STATE_ID: { @@ -36,33 +50,21 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) { // 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)); + case CAN_MSG_VALUE_SET_STATE_ID: { + msg->type = AgxMsgValueSetStateV1; + if (rx_frame->data[0] == 0xaa) + msg->body.v1_value_set_state_msg.set_neutral = true; + else + msg->body.v1_value_set_state_msg.set_neutral = false; break; } + case CAN_MSG_ACTUATOR1_STATE_ID: + case CAN_MSG_ACTUATOR2_STATE_ID: + case CAN_MSG_ACTUATOR3_STATE_ID: 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 = + rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_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)); @@ -83,10 +85,8 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { 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[2] = (int8_t)(msg->body.v1_motion_command_msg.linear); + tx_frame->data[3] = (int8_t)(msg->body.v1_motion_command_msg.angular); tx_frame->data[4] = 0; tx_frame->data[5] = 0; tx_frame->data[6] = count++; @@ -95,6 +95,22 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { break; } case AgxMsgValueSetCommandV1: { + static uint8_t count = 0; + tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID; + tx_frame->can_dlc = 8; + tx_frame->data[0] = + msg->body.v1_value_set_command_msg.set_neutral ? 0xaa : 0x00; + tx_frame->data[1] = 0x00; + tx_frame->data[2] = 0x00; + tx_frame->data[3] = 0x00; + tx_frame->data[4] = 0x00; + tx_frame->data[5] = 0x00; + tx_frame->data[6] = 0x00; + tx_frame->data[7] = CalcCanFrameChecksumV1( + tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); + break; + } + case AgxMsgLightCommand: { static uint8_t count = 0; tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID; tx_frame->can_dlc = 8; @@ -120,9 +136,6 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) { tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); break; } - case AgxMsgLightCommand: { - break; - } default: break; } diff --git a/include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h b/src/protocol_v1/agilex_protocol_v1.h similarity index 97% rename from include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h rename to src/protocol_v1/agilex_protocol_v1.h index 8a84097..163c41d 100644 --- a/include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h +++ b/src/protocol_v1/agilex_protocol_v1.h @@ -37,13 +37,15 @@ extern "C" { // CAN: control group #define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x130) #define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x140) -#define CAN_MSG_VALUE_SET_COMMAND_ID ((uint32_t)0x211) +#define CAN_MSG_VALUE_SET_COMMAND_ID ((uint32_t)0x210) // CAN: state feedback group #define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x131) #define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x141) #define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x151) +#define CAN_MSG_VALUE_SET_STATE_ID ((uint32_t)0x211) + #define CAN_MSG_ACTUATOR1_STATE_ID ((uint32_t)0x200) #define CAN_MSG_ACTUATOR2_STATE_ID ((uint32_t)0x201) #define CAN_MSG_ACTUATOR3_STATE_ID ((uint32_t)0x202) diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c index 62641b8..aad8838 100644 --- a/src/protocol_v2/agilex_msg_parser_v2.c +++ b/src/protocol_v2/agilex_msg_parser_v2.c @@ -369,7 +369,7 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) { memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); break; } - case AgxMsgSetMotionMode: { + case AgxMsgSetMotionModeCommand: { tx_frame->can_id = CAN_MSG_SET_MOTION_MODE_ID; tx_frame->can_dlc = 8; SetMotionModeFrame frame; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..2469631 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,23 @@ +# Google tests +message(STATUS "Build unit tests with Google Test.") +add_subdirectory(googletest) + +# reference: https://cliutils.gitlab.io/modern-cmake/chapters/testing/googletest.html +mark_as_advanced( + BUILD_GMOCK BUILD_GTEST BUILD_SHARED_LIBS + gmock_build_tests gtest_build_samples gtest_build_tests + gtest_disable_pthreads gtest_force_shared_crt gtest_hide_internal_symbols +) + +# add unit tests +add_executable(utests + unit_tests/utest_scout_v1_command.cpp + unit_tests/utest_hunter_v1_command.cpp) +target_link_libraries(utests PRIVATE gtest gmock gtest_main ${PROJECT_NAME}) +get_target_property(PRIVATE_HEADERS ${PROJECT_NAME} INCLUDE_DIRECTORIES) +target_include_directories(utests PRIVATE ${PRIVATE_HEADERS}) + +# gtest_discover_tests(utests) +# add_test(NAME gtest_all COMMAND utests) + +# add_subdirectory(devel) \ No newline at end of file diff --git a/test/googletest b/test/googletest new file mode 160000 index 0000000..8d51ffd --- /dev/null +++ b/test/googletest @@ -0,0 +1 @@ +Subproject commit 8d51ffdfab10b3fba636ae69bc03da4b54f8c235 diff --git a/test/unit_tests/utest_hunter_v1_command.cpp b/test/unit_tests/utest_hunter_v1_command.cpp new file mode 100644 index 0000000..fe361e2 --- /dev/null +++ b/test/unit_tests/utest_hunter_v1_command.cpp @@ -0,0 +1,106 @@ +/* + * utest_hunter_v1_command.cpp + * + * Created on: Jul 12, 2021 17:28 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include +#include +#include + +#include "gtest/gtest.h" + +#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp" +#include "protocol_v1/agilex_protocol_v1.h" + +using namespace westonrobot; + +struct HunterV1CommandTest : testing::Test { + struct can_frame frame; + HunterProtocolV1Parser parser; +}; + +TEST_F(HunterV1CommandTest, MotionLinearCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgMotionCommandV1; + msg.body.v1_motion_command_msg.linear = 0.15; + msg.body.v1_motion_command_msg.angular = 0; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x44}; + + ASSERT_EQ(frame.can_id, CAN_MSG_MOTION_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 6; ++i) { + // printf("expecting: %x, getting: %x\n", expected_data[i], frame.data[i]); + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} + +TEST_F(HunterV1CommandTest, MotionAngularCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgMotionCommandV1; + msg.body.v1_motion_command_msg.linear = 0.0; + msg.body.v1_motion_command_msg.angular = 2.55; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x01, 0x45}; + + ASSERT_EQ(frame.can_id, CAN_MSG_MOTION_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 6; ++i) { + // printf("expecting: %x, getting: %x\n", expected_data[i], frame.data[i]); + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} + +TEST_F(HunterV1CommandTest, ValueSetCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgValueSetCommandV1; + msg.body.v1_value_set_command_msg.set_neutral = true; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[6] = {0xaa, 0x00, 0x00, 0x00, 0x00, 0x00}; + + ASSERT_EQ(frame.can_id, CAN_MSG_VALUE_SET_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 6; ++i) { + // printf("expecting: %x, getting: %x\n", expected_data[i], frame.data[i]); + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} + +TEST_F(HunterV1CommandTest, LightCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgLightCommand; + msg.body.light_command_msg.enable_cmd_ctrl = true; + msg.body.light_command_msg.front_light.mode = CONST_ON; + msg.body.light_command_msg.front_light.custom_value = 100; + msg.body.light_command_msg.rear_light.mode = CUSTOM; + msg.body.light_command_msg.rear_light.custom_value = 50; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x01, 0x64, 0x03, 0x32, 0x00, 0x00, 0xe4}; + + ASSERT_EQ(frame.can_id, CAN_MSG_LIGHT_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 6; ++i) { + // printf("expecting: %x, getting: %x\n", expected_data[i], frame.data[i]); + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} \ No newline at end of file diff --git a/test/unit_tests/utest_scout_v1_command.cpp b/test/unit_tests/utest_scout_v1_command.cpp new file mode 100644 index 0000000..df9ee18 --- /dev/null +++ b/test/unit_tests/utest_scout_v1_command.cpp @@ -0,0 +1,85 @@ +/* + * utest_scout_v1_command.cpp + * + * Created on: Jul 12, 2021 16:37 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#include +#include +#include + +#include "gtest/gtest.h" + +#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp" +#include "protocol_v1/agilex_protocol_v1.h" + +using namespace westonrobot; + +struct ScoutV1CommandTest : testing::Test { + struct can_frame frame; + ScoutProtocolV1Parser parser; +}; + +TEST_F(ScoutV1CommandTest, MotionLinearCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgMotionCommandV1; + msg.body.v1_motion_command_msg.linear = 0.15; + msg.body.v1_motion_command_msg.angular = 0; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x44}; + + ASSERT_EQ(frame.can_id, CAN_MSG_MOTION_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 8; ++i) { + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} + +TEST_F(ScoutV1CommandTest, MotionAngularCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgMotionCommandV1; + msg.body.v1_motion_command_msg.linear = 0.0; + msg.body.v1_motion_command_msg.angular = 0.05235; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x01, 0x45}; + + ASSERT_EQ(frame.can_id, CAN_MSG_MOTION_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 8; ++i) { + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} + +TEST_F(ScoutV1CommandTest, LightCommandTest) { + AgxMessage msg; + + msg.type = AgxMsgLightCommand; + msg.body.light_command_msg.enable_cmd_ctrl = true; + msg.body.light_command_msg.front_light.mode = CONST_ON; + msg.body.light_command_msg.front_light.custom_value = 100; + msg.body.light_command_msg.rear_light.mode = CUSTOM; + msg.body.light_command_msg.rear_light.custom_value = 50; + + parser.EncodeMessage(&msg, &frame); + + uint8_t expected_data[8] = {0x01, 0x01, 0x64, 0x03, 0x32, 0x00, 0x00, 0xe4}; + + ASSERT_EQ(frame.can_id, CAN_MSG_LIGHT_COMMAND_ID); + ASSERT_EQ(frame.can_dlc, 8); + + for (int i = 0; i < 8; ++i) { + // printf("expecting: %x, getting: %x\n", expected_data[i], frame.data[i]); + ASSERT_EQ(frame.data[i], expected_data[i]); + } +} \ No newline at end of file