wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Unify navigation on NavigateToPose and remove legacy proxies

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 17:40:25 +08:00
parent 964d1fde67
commit 24e0b922bc
40 changed files with 461 additions and 2592 deletions
Download Patch File Download Diff File Expand all files Collapse all files
core/include/robot_bt/core.hpp
+1 -2
View File
@@ -60,7 +60,6 @@ struct GoalRequest {
std::optional<TargetBinding> target;
std::optional<PlacementBinding> placement;
std::string target_id, destination_id, shelf, posture_id;
std::string navigation_kind, navigation_ref, side, column, tier;
std::uint64_t geometry_epoch{0};
RosTime capture_after{0};
double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1};
@@ -201,7 +200,7 @@ class StageRunner {
std::uint64_t geometry_epoch_{0};
RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, holding_observed_at_{0}, empty_valid_until_{0}, empty_observed_at_{0};
std::size_t stage_index_{0};
std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_, error_code_;
std::string active_goal_, source_location_, verification_goal_, pending_posture_, detail_, error_code_;
std::optional<SteadyTime> waiting_since_;
std::optional<SteadyTime> motion_waiting_since_;
std::optional<SteadyTime> stopped_waiting_since_;
core/src/workflow.cpp
+2 -5
View File
@@ -80,10 +80,7 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) {
GoalRequest StageRunner::make_request(Stage stage,Skill skill,RosTime ros) {
GoalRequest q;q.robot_id=task_.robot_id;q.trace=task_.trace;q.trace.subtask_id=std::string(stage_name(stage))+"/"+std::to_string(++serial_);q.trace.attempt=1;q.skill=skill;q.target_id=task_.target_id;q.destination_id=task_.destination_id;q.shelf=task_.source_shelf;q.geometry_epoch=geometry_epoch_;q.capture_after=std::max(capture_after_,ros);q.position_tolerance_m=task_.position_tolerance_m;q.orientation_tolerance_rad=task_.orientation_tolerance_rad;
if(skill==Skill::NAVIGATE) { const auto& location=stage==Stage::NAVIGATE_OBSERVE?task_.observe_location:stage==Stage::NAVIGATE_SOURCE?source_location_:task_.destination_location;auto it=site_.locations.find(location);if(it==site_.locations.end()||!valid_pose(it->second))throw std::invalid_argument("navigation location is not registered with a valid pose");q.registered_pose=it->second;
if(task_.route!="LEGACY") {
q.navigation_kind="LOCATION";q.navigation_ref=location;
if(stage==Stage::NAVIGATE_SOURCE){q.navigation_kind=task_.route=="OBJECT_TABLE"?"OBJECT":"CELL";q.navigation_ref=task_.target_id;q.side=source_side_;q.column=source_column_;q.tier=source_tier_;}
}
}
if(task_.route=="LEGACY"&&(skill==Skill::EVALUATE_GRASP||skill==Skill::PICK)) { q.target=context_.target();if(!q.target||!valid_target(*q.target,task_.trace,task_.target_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("target binding stale, incomplete or mismatched"); }
if(task_.route=="LEGACY"&&skill==Skill::PLACE) { q.placement=context_.placement();if(!q.placement||!valid_placement(*q.placement,task_.trace,task_.target_id,task_.destination_id,geometry_epoch_,capture_after_,ros))throw std::invalid_argument("placement binding stale, incomplete or mismatched"); }
@@ -188,7 +185,7 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) {
if(response.tier.empty())return fail("tier required for calibrated shelf route");
key+="/"+response.tier;auto loc=site_.cell_locations.find(key),posture=site_.cell_postures.find(key);
if(loc==site_.cell_locations.end()||posture==site_.cell_postures.end()||!registered_posture(site_,posture->second))return fail("cell lacks calibrated location/posture");
source_location_=loc->second;pending_posture_=posture->second;source_side_=response.side;source_column_=response.column;source_tier_=response.tier;
source_location_=loc->second;pending_posture_=posture->second;
}else {auto it=site_.parking_locations.find(key);if(it==site_.parking_locations.end())return fail("observed shelf column has no registered parking pose");source_location_=it->second;}
}break;
case Stage::LOCALIZE_TARGET:
core/tests/navigation_readiness_test.cpp
+1 -1
View File
@@ -1,6 +1,6 @@
#include "workflow_fixture.hpp"
int main() {
for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) {
for(const auto* code:{"INPUTS_UNHEALTHY","ROBOT_STATE_UNAVAILABLE","ROBOT_ESTOP","EXECUTION_BACKEND_NOT_CONFIGURED","NAV_NOT_READY"}) {
Fixture f(std::string("navigation_")+code);auto runner=f.runner();Workflow flow(runner);
auto status=TickStatus::RUNNING;
for(unsigned i=0;i<100&&status==TickStatus::RUNNING;++i) {