SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: correct SRC1100 navigation and velocity commands

Browse Source
This commit is contained in:
xtkuang 2026-07-31 12:21:19 +08:00
parent 31b2d98625
commit f092e2539d
5 changed files with 417 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cmvr-es/devices/agv/src1100/include/src1100_agv.h
View File

@ -142,9 +142,10 @@ private:
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error); static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
static std::string extractJson_(const std::string& raw); static std::string extractJson_(const std::string& raw);
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload); static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback); static void applyMotionOptions_(
static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback); Json::Value& payload,
static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options); const AgvMotionOptions& options,
bool include_reach_options = true);
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
static AgvResult resultFromResponse_(const Json::Value& response); static AgvResult resultFromResponse_(const Json::Value& response);

79
cmvr-es/devices/agv/src1100/src/src1100_agv.cpp
View File

@ -40,6 +40,7 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022;
constexpr std::uint16_t kRobotTaskPause = 3001; constexpr std::uint16_t kRobotTaskPause = 3001;
constexpr std::uint16_t kRobotTaskResume = 3002; constexpr std::uint16_t kRobotTaskResume = 3002;
constexpr std::uint16_t kRobotTaskCancel = 3003; constexpr std::uint16_t kRobotTaskCancel = 3003;
constexpr std::uint16_t kRobotTaskGoPoint = 3050;
constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTarget = 3051;
constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
constexpr std::uint16_t kRobotConfigLock = 4005; constexpr std::uint16_t kRobotConfigLock = 4005;
@ -624,18 +625,19 @@ AgvResult Src1100Agv::navigateToPose(
const AgvMotionOptions& options, const AgvMotionOptions& options,
const AgvAdapterParams& adapter_params) const AgvAdapterParams& adapter_params)
{ {
(void)adapter_params;
// API 3050 is the controller's arbitrary world-coordinate navigation
// command. Do not encode a map pose as API 3051/freeGo: that extension is
// only defined for differential-drive chassis, and a multi-steer chassis
// may accept the command before the navigation task fails.
Json::Value payload(Json::objectValue); Json::Value payload(Json::objectValue);
jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); jsonMember(payload, "x") = pose.x;
jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); jsonMember(payload, "y") = pose.y;
jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); jsonMember(payload, "angle") = pose.theta;
auto& free_go = jsonMember(payload, "freeGo"); applyMotionOptions_(payload, options, false);
jsonMember(free_go, "x") = pose.x;
jsonMember(free_go, "y") = pose.y;
jsonMember(free_go, "theta") = pose.theta;
applyMotionOptions_(payload, options);
applyAdapterParams_(payload, adapter_params);
Json::Value response; Json::Value response;
auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoPoint, payload, &response);
return result.ok() ? resultFromResponse_(response) : result; return result.ok() ? resultFromResponse_(response) : result;
} }
@ -647,8 +649,10 @@ AgvResult Src1100Agv::navigateToStation(
Json::Value payload(Json::objectValue); Json::Value payload(Json::objectValue);
jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION");
jsonMember(payload, "id") = station_id; jsonMember(payload, "id") = station_id;
applyMotionOptions_(payload, options);
applyAdapterParams_(payload, adapter_params); applyAdapterParams_(payload, adapter_params);
// Canonical typed motion options must win over string-valued adapter
// extensions so the SRC controller receives JSON numbers.
applyMotionOptions_(payload, options);
Json::Value response; Json::Value response;
auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response);
return result.ok() ? resultFromResponse_(response) : result; return result.ok() ? resultFromResponse_(response) : result;
@ -711,7 +715,6 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity)
jsonMember(payload, "vx") = velocity.vx; jsonMember(payload, "vx") = velocity.vx;
jsonMember(payload, "vy") = velocity.vy; jsonMember(payload, "vy") = velocity.vy;
jsonMember(payload, "w") = velocity.wz; jsonMember(payload, "w") = velocity.wz;
jsonMember(payload, "duration") = -1;
Json::Value response; Json::Value response;
auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response);
return result.ok() ? resultFromResponse_(response) : result; return result.ok() ? resultFromResponse_(response) : result;
@ -1949,40 +1952,45 @@ AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std:
return AgvResult::success(); return AgvResult::success();
} }
int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) void Src1100Agv::applyMotionOptions_(
{ Json::Value& payload,
const auto value = params.getDouble(key); const AgvMotionOptions& options,
return value ? static_cast<int>(*value) : fallback; const bool include_reach_options)
}
double Src1100Agv::optionalDouble_(const AgvAdapterParams& params, const std::string& key, const double fallback)
{
const auto value = params.getDouble(key);
return value ? *value : fallback;
}
void Src1100Agv::applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options)
{ {
if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed;
if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed;
if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration;
if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration;
if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; if (include_reach_options) {
if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance;
if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle;
}
} }
void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params)
{ {
for (const auto& [key, value] : params.values) { for (const auto& [key, value] : params.values) {
if (key.rfind("port_", 0) == 0) { if (key.rfind("port_", 0) == 0
|| key == "target_id"
|| key == "id"
|| key == "x"
|| key == "y"
|| key == "angle"
|| key == "freeGo"
|| key == "max_speed"
|| key == "max_wspeed"
|| key == "max_acc"
|| key == "max_wacc"
|| key == "reach_dist"
|| key == "reach_angle"
|| key == "jack_height") {
continue; continue;
} }
jsonMember(payload, key) = value; jsonMember(payload, key) = value;
} }
jsonMember(payload, "jack_height") = optionalDouble_( if (const auto jack_height = params.getDouble("jack_height")) {
params, jsonMember(payload, "jack_height") = *jack_height;
"jack_height", }
jsonGet(payload, "jack_height", 0.0).asDouble());
} }
AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response)
@ -1992,8 +2000,11 @@ AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response)
if (ret_code == 0) { if (ret_code == 0) {
return AgvResult::success(); return AgvResult::success();
} }
return AgvResult::failure(AgvErrorCode::CommandFailed, std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(ret_code);
message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); if (!message.empty()) {
detail += ", err_msg=" + message;
}
return AgvResult::failure(AgvErrorCode::CommandFailed, detail);
} }
} // namespace cmvr::device } // namespace cmvr::device

175
cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp
View File

@ -47,6 +47,7 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022;
constexpr std::uint16_t kRobotTaskPause = 3001; constexpr std::uint16_t kRobotTaskPause = 3001;
constexpr std::uint16_t kRobotTaskResume = 3002; constexpr std::uint16_t kRobotTaskResume = 3002;
constexpr std::uint16_t kRobotTaskCancel = 3003; constexpr std::uint16_t kRobotTaskCancel = 3003;
constexpr std::uint16_t kRobotTaskGoPoint = 3050;
constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTarget = 3051;
constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
constexpr std::uint16_t kRobotConfigLock = 4005; constexpr std::uint16_t kRobotConfigLock = 4005;
@ -68,6 +69,39 @@ struct CommandRecord {
std::string payload; std::string payload;
}; };
Json::Value parsePayload(const CommandRecord& record)
{
Json::Value payload;
Json::CharReaderBuilder builder;
std::string error;
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
if (!reader->parse(
record.payload.data(),
record.payload.data() + record.payload.size(),
&payload,
&error)) {
ADD_FAILURE() << "Failed to parse command " << record.command
<< " payload: " << error;
}
return payload;
}
const Json::Value& payloadValue(const Json::Value& payload, const char* key)
{
const auto* value = payload.find(key, key + std::strlen(key));
if (!value) {
ADD_FAILURE() << "Missing JSON field: " << key;
static const Json::Value null_value;
return null_value;
}
return *value;
}
bool payloadHas(const Json::Value& payload, const char* key)
{
return payload.find(key, key + std::strlen(key)) != nullptr;
}
bool receiveExact(const int fd, void* output, const std::size_t size) bool receiveExact(const int fd, void* output, const std::size_t size)
{ {
auto* bytes = static_cast<std::uint8_t*>(output); auto* bytes = static_cast<std::uint8_t*>(output);
@ -318,7 +352,7 @@ protected:
TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst)
{ {
expectControlled(kRobotTaskGoTarget, [this]() { expectControlled(kRobotTaskGoPoint, [this]() {
return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
}); });
expectControlled(kRobotTaskGoTarget, [this]() { expectControlled(kRobotTaskGoTarget, [this]() {
@ -369,6 +403,7 @@ TEST_F(Src1100ControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand)
EXPECT_FALSE(result.ok()); EXPECT_FALSE(result.ok());
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos);
EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos);
const auto records = controller_.records(); const auto records = controller_.records();
ASSERT_EQ(records.size(), 1U); ASSERT_EQ(records.size(), 1U);
EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[0].command, kRobotConfigLock);
@ -384,6 +419,7 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendSt
EXPECT_FALSE(result.ok()); EXPECT_FALSE(result.ok());
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos);
EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos);
const auto records = controller_.records(); const auto records = controller_.records();
ASSERT_EQ(records.size(), 1U); ASSERT_EQ(records.size(), 1U);
EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[0].command, kRobotConfigLock);
@ -400,6 +436,8 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesF
EXPECT_FALSE(result.ok()); EXPECT_FALSE(result.ok());
EXPECT_NE(result.message.find("control stop"), std::string::npos); EXPECT_NE(result.message.find("control stop"), std::string::npos);
EXPECT_NE(result.message.find("cancel navigation"), std::string::npos); EXPECT_NE(result.message.find("cancel navigation"), std::string::npos);
EXPECT_NE(result.message.find("ret_code=50001"), std::string::npos);
EXPECT_NE(result.message.find("ret_code=50002"), std::string::npos);
const auto records = controller_.records(); const auto records = controller_.records();
ASSERT_EQ(records.size(), 3U); ASSERT_EQ(records.size(), 3U);
EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[0].command, kRobotConfigLock);
@ -431,5 +469,140 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority)
EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); EXPECT_EQ(records[0].command, kRobotConfigDownloadMap);
} }
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimits)
{
AgvMotionOptions options;
options.max_speed = 0.6;
options.max_angular_speed = 0.7;
options.max_acceleration = 0.8;
options.max_angular_acceleration = 0.9;
options.reach_distance = 0.1;
options.reach_angle = 0.2;
controller_.clearRecords();
const auto result = agv_->navigateToPose(
math::Pose2d{1.0, 2.0, 0.5},
options);
ASSERT_TRUE(result.ok()) << result.message;
const auto records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[0].command, kRobotConfigLock);
EXPECT_EQ(records[1].command, kRobotTaskGoPoint);
const auto payload = parsePayload(records[1]);
EXPECT_TRUE(payloadValue(payload, "x").isNumeric());
EXPECT_TRUE(payloadValue(payload, "y").isNumeric());
EXPECT_TRUE(payloadValue(payload, "angle").isNumeric());
EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0);
EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0);
EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").asDouble(), 0.5);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9);
EXPECT_FALSE(payloadHas(payload, "id"));
EXPECT_FALSE(payloadHas(payload, "source_id"));
EXPECT_FALSE(payloadHas(payload, "skill_name"));
EXPECT_FALSE(payloadHas(payload, "freeGo"));
EXPECT_FALSE(payloadHas(payload, "reach_dist"));
EXPECT_FALSE(payloadHas(payload, "reach_angle"));
EXPECT_FALSE(payloadHas(payload, "jack_height"));
}
TEST_F(Src1100ControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions)
{
AgvMotionOptions options;
options.max_speed = 0.4;
options.max_angular_speed = 0.5;
options.max_acceleration = 0.6;
options.max_angular_acceleration = 0.7;
AgvAdapterParams adapter_params;
adapter_params.values.emplace("id", "wrong-station");
adapter_params.values.emplace("x", "99.0");
adapter_params.values.emplace("freeGo", "invalid");
adapter_params.values.emplace("max_speed", "not-a-number");
adapter_params.values.emplace("reach_dist", "not-a-number");
controller_.clearRecords();
const auto result = agv_->navigateToStation(
"station-1",
options,
adapter_params);
ASSERT_TRUE(result.ok()) << result.message;
const auto records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[0].command, kRobotConfigLock);
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
const auto payload = parsePayload(records[1]);
EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION");
EXPECT_EQ(payloadValue(payload, "id").asString(), "station-1");
EXPECT_TRUE(payloadValue(payload, "max_speed").isNumeric());
EXPECT_TRUE(payloadValue(payload, "max_wspeed").isNumeric());
EXPECT_TRUE(payloadValue(payload, "max_acc").isNumeric());
EXPECT_TRUE(payloadValue(payload, "max_wacc").isNumeric());
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.4);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.5);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.6);
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.7);
EXPECT_FALSE(payloadHas(payload, "x"));
EXPECT_FALSE(payloadHas(payload, "freeGo"));
EXPECT_FALSE(payloadHas(payload, "reach_dist"));
EXPECT_FALSE(payloadHas(payload, "jack_height"));
}
TEST_F(Src1100ControlAuthorityTest, SetVelocityUsesOnlyDocumentedNumericFields)
{
controller_.clearRecords();
const auto result = agv_->setVelocity(AgvVelocity{0.1, -0.2, 0.3});
ASSERT_TRUE(result.ok()) << result.message;
auto records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[0].command, kRobotConfigLock);
EXPECT_EQ(records[1].command, kRobotControlMotion);
auto payload = parsePayload(records[1]);
EXPECT_TRUE(payloadValue(payload, "vx").isNumeric());
EXPECT_TRUE(payloadValue(payload, "vy").isNumeric());
EXPECT_TRUE(payloadValue(payload, "w").isNumeric());
EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.1);
EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), -0.2);
EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.3);
EXPECT_FALSE(payloadHas(payload, "duration"));
controller_.clearRecords();
const auto stop_result = agv_->stopVelocityControl();
ASSERT_TRUE(stop_result.ok()) << stop_result.message;
records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[1].command, kRobotControlMotion);
payload = parsePayload(records[1]);
EXPECT_TRUE(payloadValue(payload, "vx").isNumeric());
EXPECT_TRUE(payloadValue(payload, "vy").isNumeric());
EXPECT_TRUE(payloadValue(payload, "w").isNumeric());
EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.0);
EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), 0.0);
EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.0);
EXPECT_FALSE(payloadHas(payload, "duration"));
}
TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessage)
{
controller_.setResponseCode(kRobotControlMotion, 41200);
controller_.clearRecords();
const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0});
EXPECT_FALSE(result.ok());
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
EXPECT_NE(result.message.find("ret_code=41200"), std::string::npos);
EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
}
} // namespace } // namespace
} // namespace cmvr::device } // namespace cmvr::device

31
cmvr-es/service/CMakeLists.txt
View File

@ -133,6 +133,37 @@ if(BUILD_TESTING)
ENVIRONMENT "${_grpc_motor_test_environment}" ENVIRONMENT "${_grpc_motor_test_environment}"
) )
add_executable(grpc_agv_service_test
grpc/tests/grpc_agv_service_test.cpp
)
target_include_directories(grpc_agv_service_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
)
target_link_libraries(grpc_agv_service_test
PRIVATE
service
gtest
gtest_main
pthread
)
add_test(
NAME grpc_agv_service_test
COMMAND grpc_agv_service_test
)
set(_grpc_agv_test_environment
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
list(APPEND _grpc_agv_test_environment
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
endif()
set_tests_properties(grpc_agv_service_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_grpc_agv_test_environment}"
)
set(_grpc_motor_modbus_e2e_libmodbus_root set(_grpc_motor_modbus_e2e_libmodbus_root
"${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11")
add_executable(grpc_motor_service_modbus_e2e_test add_executable(grpc_motor_service_modbus_e2e_test

163
cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp Normal file
View File

@ -0,0 +1,163 @@
#include "service/grpc/include/grpc_agv_service.h"
#include <memory>
#include <string>
#include <grpcpp/grpcpp.h>
#include <gtest/gtest.h>
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "manager/device_manager/include/device_manager.h"
namespace cmvr::service {
namespace {
constexpr char kNativeErrorMessage[] =
"SRC1100 command failed: ret_code=41200, err_msg=speed_illegal";
class FakeAgv final : public device::AbstractAGV {
public:
FakeAgv()
{
id_ = "test-agv";
}
std::string typeName() const override { return "FakeAgv"; }
device::AgvResult navigateToPose(
const math::Pose2d& pose,
const device::AgvMotionOptions& options,
const device::AgvAdapterParams&) override
{
pose_ = pose;
pose_options_ = options;
return device::AgvResult::success();
}
device::AgvResult navigateToStation(
const std::string& station_id,
const device::AgvMotionOptions& options,
const device::AgvAdapterParams&) override
{
station_id_ = station_id;
station_options_ = options;
return device::AgvResult::success();
}
device::AgvResult setVelocity(const device::AgvVelocity&) override
{
return device::AgvResult::failure(
device::AgvErrorCode::CommandFailed,
kNativeErrorMessage);
}
math::Pose2d pose_;
device::AgvMotionOptions pose_options_;
std::string station_id_;
device::AgvMotionOptions station_options_;
};
class GrpcAgvServiceTest : public ::testing::Test {
protected:
void SetUp() override
{
config::DeviceManagerConfig config;
auto& manager = device::DeviceManager::getInstance(config);
agv_ = std::make_shared<FakeAgv>();
manager.registerDevice(agv_);
service_ = std::make_unique<gRPCAgvServiceImpl>();
}
void TearDown() override
{
service_.reset();
agv_.reset();
device::DeviceManager::destroyInstance();
}
std::shared_ptr<FakeAgv> agv_;
std::unique_ptr<gRPCAgvServiceImpl> service_;
};
void setMotionOptions(msgs::AgvMotionOptions* options)
{
options->set_max_speed(0.4);
options->set_max_angular_speed(0.5);
options->set_max_acceleration(0.6);
options->set_max_angular_acceleration(0.7);
options->set_reach_distance(0.08);
options->set_reach_angle(0.09);
}
void expectMotionOptions(const device::AgvMotionOptions& options)
{
EXPECT_DOUBLE_EQ(options.max_speed, 0.4);
EXPECT_DOUBLE_EQ(options.max_angular_speed, 0.5);
EXPECT_DOUBLE_EQ(options.max_acceleration, 0.6);
EXPECT_DOUBLE_EQ(options.max_angular_acceleration, 0.7);
EXPECT_DOUBLE_EQ(options.reach_distance, 0.08);
EXPECT_DOUBLE_EQ(options.reach_angle, 0.09);
}
TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions)
{
api::AgvNavigateToPoseCommand_Request pose_request;
pose_request.mutable_header()->set_device_id("test-agv");
pose_request.mutable_pose()->set_x(1.0);
pose_request.mutable_pose()->set_y(2.0);
pose_request.mutable_pose()->set_theta(0.5);
setMotionOptions(pose_request.mutable_options());
api::AgvNavigateToPoseCommand_Feedback pose_response;
grpc::ServerContext pose_context;
const auto pose_status = service_->navigateToPose(
&pose_context,
&pose_request,
&pose_response);
ASSERT_TRUE(pose_status.ok()) << pose_status.error_message();
EXPECT_TRUE(pose_response.header().success());
EXPECT_DOUBLE_EQ(agv_->pose_.x, 1.0);
EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0);
EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5);
expectMotionOptions(agv_->pose_options_);
api::AgvNavigateToStationCommand_Request station_request;
station_request.mutable_header()->set_device_id("test-agv");
station_request.set_station_id("station-1");
setMotionOptions(station_request.mutable_options());
api::AgvNavigateToStationCommand_Feedback station_response;
grpc::ServerContext station_context;
const auto station_status = service_->navigateToStation(
&station_context,
&station_request,
&station_response);
ASSERT_TRUE(station_status.ok()) << station_status.error_message();
EXPECT_TRUE(station_response.header().success());
EXPECT_EQ(agv_->station_id_, "station-1");
expectMotionOptions(agv_->station_options_);
}
TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage)
{
api::AgvSetVelocityCommand_Request request;
request.mutable_header()->set_device_id("test-agv");
request.mutable_velocity()->set_vx(0.1);
api::AgvSetVelocityCommand_Feedback response;
grpc::ServerContext context;
const auto status = service_->setVelocity(
&context,
&request,
&response);
EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
EXPECT_EQ(status.error_message(), kNativeErrorMessage);
EXPECT_FALSE(response.header().success());
EXPECT_EQ(response.header().error_message(), kNativeErrorMessage);
}
} // namespace
} // namespace cmvr::service