From f092e2539d1697b972e122dcfb7aace62ce04e44 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 12:21:19 +0800 Subject: [PATCH] fix: correct SRC1100 navigation and velocity commands --- .../devices/agv/src1100/include/src1100_agv.h | 7 +- .../devices/agv/src1100/src/src1100_agv.cpp | 79 ++++---- .../tests/src1100_control_authority_test.cpp | 175 +++++++++++++++++- cmvr-es/service/CMakeLists.txt | 31 ++++ .../grpc/tests/grpc_agv_service_test.cpp | 163 ++++++++++++++++ 5 files changed, 417 insertions(+), 38 deletions(-) create mode 100644 cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 466c6ced..3d745590 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -142,9 +142,10 @@ private: static bool parseJson_(const std::string& input, Json::Value& output, std::string& error); static std::string extractJson_(const std::string& raw); 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 double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback); - static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options); + static void applyMotionOptions_( + Json::Value& payload, + const AgvMotionOptions& options, + bool include_reach_options = true); static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); static AgvResult resultFromResponse_(const Json::Value& response); diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index eb26fdc2..1158473a 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -40,6 +40,7 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022; constexpr std::uint16_t kRobotTaskPause = 3001; constexpr std::uint16_t kRobotTaskResume = 3002; constexpr std::uint16_t kRobotTaskCancel = 3003; +constexpr std::uint16_t kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -624,18 +625,19 @@ AgvResult Src1100Agv::navigateToPose( const AgvMotionOptions& options, 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); - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); - jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); - auto& free_go = jsonMember(payload, "freeGo"); - 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); + jsonMember(payload, "x") = pose.x; + jsonMember(payload, "y") = pose.y; + jsonMember(payload, "angle") = pose.theta; + applyMotionOptions_(payload, options, false); 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; } @@ -647,8 +649,10 @@ AgvResult Src1100Agv::navigateToStation( Json::Value payload(Json::objectValue); jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); jsonMember(payload, "id") = station_id; - applyMotionOptions_(payload, options); 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; auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; @@ -711,7 +715,6 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) jsonMember(payload, "vx") = velocity.vx; jsonMember(payload, "vy") = velocity.vy; jsonMember(payload, "w") = velocity.wz; - jsonMember(payload, "duration") = -1; Json::Value response; auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); return result.ok() ? resultFromResponse_(response) : result; @@ -1949,40 +1952,45 @@ AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std: return AgvResult::success(); } -int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) -{ - const auto value = params.getDouble(key); - return value ? static_cast(*value) : fallback; -} - -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) +void Src1100Agv::applyMotionOptions_( + Json::Value& payload, + const AgvMotionOptions& options, + const bool include_reach_options) { 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_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.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; - if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; + if (include_reach_options) { + 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) { 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; } jsonMember(payload, key) = value; } - jsonMember(payload, "jack_height") = optionalDouble_( - params, - "jack_height", - jsonGet(payload, "jack_height", 0.0).asDouble()); + if (const auto jack_height = params.getDouble("jack_height")) { + jsonMember(payload, "jack_height") = *jack_height; + } } AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) @@ -1992,8 +2000,11 @@ AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) if (ret_code == 0) { return AgvResult::success(); } - return AgvResult::failure(AgvErrorCode::CommandFailed, - message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); + std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(ret_code); + if (!message.empty()) { + detail += ", err_msg=" + message; + } + return AgvResult::failure(AgvErrorCode::CommandFailed, detail); } } // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp index cc834dd1..f83b37ba 100644 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -47,6 +47,7 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022; constexpr std::uint16_t kRobotTaskPause = 3001; constexpr std::uint16_t kRobotTaskResume = 3002; constexpr std::uint16_t kRobotTaskCancel = 3003; +constexpr std::uint16_t kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -68,6 +69,39 @@ struct CommandRecord { std::string payload; }; +Json::Value parsePayload(const CommandRecord& record) +{ + Json::Value payload; + Json::CharReaderBuilder builder; + std::string error; + std::unique_ptr 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) { auto* bytes = static_cast(output); @@ -318,7 +352,7 @@ protected: TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { - expectControlled(kRobotTaskGoTarget, [this]() { + expectControlled(kRobotTaskGoPoint, [this]() { return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); }); expectControlled(kRobotTaskGoTarget, [this]() { @@ -369,6 +403,7 @@ TEST_F(Src1100ControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); 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(); ASSERT_EQ(records.size(), 1U); EXPECT_EQ(records[0].command, kRobotConfigLock); @@ -384,6 +419,7 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendSt EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); 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(); ASSERT_EQ(records.size(), 1U); EXPECT_EQ(records[0].command, kRobotConfigLock); @@ -400,6 +436,8 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesF EXPECT_FALSE(result.ok()); 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("ret_code=50001"), std::string::npos); + EXPECT_NE(result.message.find("ret_code=50002"), std::string::npos); const auto records = controller_.records(); ASSERT_EQ(records.size(), 3U); EXPECT_EQ(records[0].command, kRobotConfigLock); @@ -431,5 +469,140 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) 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 cmvr::device diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 6da68349..adccab30 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -133,6 +133,37 @@ if(BUILD_TESTING) 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 "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") add_executable(grpc_motor_service_modbus_e2e_test diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp new file mode 100644 index 00000000..6a3077a8 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -0,0 +1,163 @@ +#include "service/grpc/include/grpc_agv_service.h" + +#include +#include + +#include +#include + +#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(); + manager.registerDevice(agv_); + service_ = std::make_unique(); + } + + void TearDown() override + { + service_.reset(); + agv_.reset(); + device::DeviceManager::destroyInstance(); + } + + std::shared_ptr agv_; + std::unique_ptr 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