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

Add AGV relocalization support and fix task_type handling in synchronous navigation

Browse Source
This commit is contained in:
linbo 2026-08-19 11:17:37 +08:00
parent aac719a932
commit abe6486e43
12 changed files with 166 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cmvr-es/devices/agv/abstract_agv.h
View File

@ -70,6 +70,13 @@ public:
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented");
}
virtual AgvResult relocalize(const math::Pose2d& pose)
{
(void)pose;
return AgvResult::failure(AgvErrorCode::UnsupportedCommand,"relocalize not implemented");
}
/**
* @brief 发起到世界/地图位姿的导航任务。
*/

4
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h
View File

@ -48,6 +48,8 @@ public:
AgvResult emergencyStop() override;
AgvResult clearFault() override;
AgvResult relocalize(const math::Pose2d& pose) override;
AgvResult navigateToPose(
const math::Pose2d& pose,
@ -83,6 +85,8 @@ public:
AgvUnifiedMapUpdate& update) const override;
AgvResult stopMapping() override;
private:
friend class SeerRobokitAgvTestPeer;

33
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h
View File

@ -56,6 +56,39 @@ static inline bool globalTaskStateIsKnownTerminal(const int state)
return state == 0 || exactTaskStateIsKnownTerminal(state);
}
// Some SRC firmware versions report station navigation as task type 3.
// Exact task ids and target ids remain the primary ownership evidence.
static inline bool exactTrackedNavigationTaskTypeMatches(
const AgvTaskType expected_type,
const int actual_type)
{
switch (expected_type) {
case AgvTaskType::NavigateToPose:
return actual_type == 1;
case AgvTaskType::NavigateToStation:
case AgvTaskType::FollowPath:
return actual_type == 2 || actual_type == 3;
default:
return false;
}
}
static inline bool globalNavigationTaskTypeMatches(
const AgvTaskType expected_type,
const int actual_type)
{
switch (expected_type) {
case AgvTaskType::NavigateToPose:
return actual_type == 1;
case AgvTaskType::NavigateToStation:
return actual_type == 2 || actual_type == 3;
case AgvTaskType::FollowPath:
return actual_type == 3;
default:
return false;
}
}
static inline double angleDistance(const double lhs, const double rhs)
{
return std::abs(std::remainder(lhs - rhs, kTwoPi));

1
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h
View File

@ -15,6 +15,7 @@ constexpr std::uint16_t kRobotStatusStation = 1301;
constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
constexpr std::uint16_t kRobotStatusDownloadFile = 1800;
constexpr std::uint16_t kRobotControlStop = 2000;
constexpr std::uint16_t kRobotControlReloc = 2002;
constexpr std::uint16_t kRobotControlMotion = 2010;
constexpr std::uint16_t kRobotControlLoadMap = 2022;
constexpr std::uint16_t kRobotTaskPause = 3001;

15
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
View File

@ -127,15 +127,12 @@ AgvResult SeerRobokitAgv::sendControlledCommand_(
"because 1101 ownership preflight was unavailable: "
+ snapshot_result.message);
}
const int expected_type = expected_active_navigation->type
== AgvTaskType::NavigateToPose
? 1
: (expected_active_navigation->type
== AgvTaskType::NavigateToStation
? 2
: 3);
const bool global_active =
exactTaskStateIsActive(snapshot.task_status);
const bool global_type_matches =
globalNavigationTaskTypeMatches(
expected_active_navigation->type,
snapshot.task_type);
const bool target_conflicts = global_active
&& !snapshot.target_id.empty()
&& !expected_active_navigation->target_ids.empty()
@ -145,7 +142,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_(
snapshot.target_id)
== expected_active_navigation->target_ids.end();
if (global_active
&& (snapshot.task_type != expected_type
&& (!global_type_matches
|| target_conflicts)) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
@ -167,7 +164,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_(
if (globalTaskStateIsKnownTerminal(snapshot.task_status)
&& snapshot.task_status != 0
&& !clearing_path_queue
&& snapshot.task_type == expected_type
&& global_type_matches
&& terminal_target_matches) {
return AgvResult::success();
}

34
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
View File

@ -170,6 +170,40 @@ AgvResult SeerRobokitAgv::clearFault()
return result.ok() ? resultFromResponse_(response) : result;
}
AgvResult SeerRobokitAgv::relocalize(const math::Pose2d& pose)
{
if (!std::isfinite(pose.x)
|| !std::isfinite(pose.y)
|| !std::isfinite(pose.theta)) {
return AgvResult::failure(
AgvErrorCode::InvalidArgument,
"SEER Robokit relocalization pose x, y, and theta must be finite");
}
Json::Value payload(Json::objectValue);
jsonMember(payload, "x") = pose.x;
jsonMember(payload, "y") = pose.y;
jsonMember(payload, "angle") = pose.theta;
Json::Value response;
std::uint64_t accepted_generation = 0;
auto result = sendControlledCommand_(
sock_control_,
kRobotControlReloc,
payload,
&response,
&accepted_generation);
if (accepted_generation != 0) {
clearPoseTask_(accepted_generation);
}
return result;
}
AgvResult SeerRobokitAgv::navigateToPose(
const math::Pose2d& pose,
const AgvMotionOptions& options,

24
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp
View File

@ -618,10 +618,10 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_(
+ snapshot_result.message);
}
last_detail += ", " + snapshot.detail;
const int expected_global_type =
context.type == AgvTaskType::NavigateToPose
? 1
: (context.type == AgvTaskType::NavigateToStation ? 2 : 3);
const bool global_type_matches =
globalNavigationTaskTypeMatches(
context.type,
snapshot.task_type);
const bool global_target_matches = snapshot.target_id.empty()
|| context.target_ids.empty()
|| std::find(
@ -632,7 +632,7 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_(
const bool global_terminal = snapshot.task_status_present &&
(snapshot.task_status == 0
|| (exactTaskStateIsKnownTerminal(snapshot.task_status)
&& snapshot.task_type == expected_global_type
&& global_type_matches
&& global_target_matches));
const bool task_termination_confirmed = require_global_stopped
? (!any_exact_task_active && global_terminal)
@ -915,9 +915,9 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
}
if (task_status.type_present) {
const bool expected_type =
context.type == AgvTaskType::NavigateToStation
? task_status.type == 2
: (task_status.type == 2 || task_status.type == 3);
exactTrackedNavigationTaskTypeMatches(
context.type,
task_status.type);
if (!expected_type) {
return failAndCancelTrackedNavigation_(
context,
@ -1090,8 +1090,6 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
continue;
}
const int expected_global_type =
context.type == AgvTaskType::NavigateToStation ? 2 : 3;
const bool global_active = snapshot.task_status >= 1
&& snapshot.task_status <= 3;
// 1110 and 1101 are separate controller publications. During the
@ -1104,7 +1102,9 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
const bool attributed_global_active = global_attribution_ready
&& global_active;
const bool global_type_matches =
snapshot.task_type == expected_global_type;
globalNavigationTaskTypeMatches(
context.type,
snapshot.task_type);
const bool global_target_matches =
context.type == AgvTaskType::NavigateToStation
? (!snapshot.target_id.empty()
@ -1185,7 +1185,7 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
const bool expected_completed_snapshot =
snapshot.task_status == 0
|| (snapshot.task_status == 4
&& snapshot.task_type == expected_global_type
&& global_type_matches
&& (context.target_id.empty()
|| snapshot.target_id.empty()
|| snapshot.target_id == context.target_id));

5
cmvr-es/service/grpc/server/include/grpc_agv_service.h
View File

@ -83,6 +83,11 @@ public:
const api::AgvTranslateCommand_Request* request,
api::AgvTranslateCommand_Feedback* response) override;
grpc::Status relocalize(grpc::ServerContext* context,
const api::AgvRelocalizeCommand_Request* request,
api::AgvRelocalizeCommand_Feedback* response) override;
private:
device::DeviceManager& dmgr_;
std::shared_ptr<GrpcSecurityGateway> security_gateway_;

41
cmvr-es/service/grpc/server/src/grpc_agv_service.cpp
View File

@ -722,6 +722,47 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext* context,
});
}
grpc::Status gRPCAgvServiceImpl::relocalize(
grpc::ServerContext* context,
const api::AgvRelocalizeCommand_Request* request,
api::AgvRelocalizeCommand_Feedback* response)
{
return executeRegisteredGrpcCommand(
security_gateway_, context, dmgr_.safetyManager(),
"/cmvr.api.AgvService/relocalize", request, response,
[this, request, response](GrpcCommandTransaction& command) {
const std::string device_id = request->header().device_id();
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
if (!agv) {
return setDeviceNotFound(response, device_id);
}
ScopedUnaryAgvControlLease control_lease(
device_id, "relocalize");
if (!control_lease.acquired()) {
return setControlAdmissionFailure(
response, device_id, control_lease);
}
auto dispatch = control_lease.tryBeginDispatch();
if (!dispatch.acquired()) {
return setControlDispatchFailure(
response, device_id, control_lease, "relocalize");
}
if (!command.beginDispatch()) {
return command.dispatchStatus();
}
return setResponseResult(
response,
agv->relocalize(toPose2d(request->pose())));
});
}
grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context,
const api::AgvNavigateToPoseCommand_Request* request,
api::AgvNavigateToPoseCommand_Feedback* response)

2
cmvr-es/service/grpc/server/src/grpc_security.cpp
View File

@ -466,7 +466,7 @@ const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry()
GrpcAccessClass::Mutate, CommandIntent::Actuate,
SafetyPolicyFamily::Control, true);
add_many("AgvService",
{"switchMap", "uploadMap", "startMapping"},
{"switchMap", "uploadMap", "startMapping", "relocalize"},
GrpcAccessClass::Mutate, CommandIntent::Configure,
SafetyPolicyFamily::Control, true);

17
protos/cmvr/api/agv_command.proto
View File

@ -37,6 +37,23 @@ message AgvNavigationStatusCommand {
}
}
// 在当前活动地图中重新设置 AGV 定位位姿。
message AgvRelocalizeCommand {
message Request {
// header.device_id 指定目标 AGV 设备。
CommandHeader.Request header = 1;
// x/y 单位：米，theta 单位：弧度，逆时针为正。
cmvr.msgs.AgvPose2d pose = 2;
}
message Feedback {
// 成功仅表示控制器接受命令，不表示定位已经收敛。
CommandHeader.Feedback header = 1;
}
}
// 导航到指定地图位姿命令。
message AgvNavigateToPoseCommand {
// 请求体。

3
protos/cmvr/api/agv_service.proto
View File

@ -20,6 +20,9 @@ service AgvService {
// 清除可恢复故障或告警。
rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback);
// 在当前活动地图中重新设置定位位姿。调用前 AGV 必须已经停止。
rpc relocalize(AgvRelocalizeCommand.Request)returns (AgvRelocalizeCommand.Feedback);
// 导航到指定地图位姿。目标位姿 x/y 单位为米，theta 单位为弧度。
rpc navigateToPose(AgvNavigateToPoseCommand.Request) returns (AgvNavigateToPoseCommand.Feedback);