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

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-12 19:02:41 +08:00
parent 10d9fc734e
commit 6cbdebc38a
14 changed files with 337 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored Normal file
View File

@@ -0,0 +1,3 @@
[submodule "googletest"]
path = test/googletest
url = https://github.com/google/googletest.git

41
CMakeLists.txt
View File

@@ -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
$<INSTALL_INTERFACE:include>
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")

11
include/ugv_sdk/details/interface/agilex_message.h
View File

@@ -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;

40
include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp → include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp
View File

@@ -17,7 +17,7 @@
namespace westonrobot {
template <typename RobotLimitsType>
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<int8_t>(
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<ScoutV2Limits>;
using ScoutMiniProtocolV1Parser = DiffDriveModelV1Parser<ScoutV2Limits>;
using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
using ScoutMiniProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
using HunterProtocolV1Parser = ProtocolV1Parser<HunterV1Limits>;
} // namespace westonrobot
#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */

23
include/ugv_sdk/details/protocol_v1/robot_limits.hpp
View File

@@ -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

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

@@ -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

2
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -114,7 +114,7 @@ class ScoutBase : public AgilexBase<Parser>, 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 {

87
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -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;
}

4
include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h → src/protocol_v1/agilex_protocol_v1.h
View File

@@ -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)

2
src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -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;

23
test/CMakeLists.txt Normal file
View File

@@ -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)

1
test/googletest Submodule

Submodule test/googletest added at 8d51ffdfab

106
test/unit_tests/utest_hunter_v1_command.cpp Normal file
View File

@@ -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 <stdio.h>
#include <vector>
#include <iostream>
#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]);
}
}

85
test/unit_tests/utest_scout_v1_command.cpp Normal file
View File

@@ -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 <stdio.h>
#include <vector>
#include <iostream>
#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]);
}
}