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
ros2/bt_executor/tools/native_navigation_probe.cpp
+3 -3
View File
@@ -20,7 +20,7 @@ int main(int argc,char** argv) {
while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()<until) {
rclcpp::spin_some(node); std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("Navigate discovery timed out");
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("NavigateToPose discovery timed out");
robot_bt::GoalRequest request;
request.goal_id="probe-"+std::to_string(scenario);request.robot_id="robot_01";
request.trace.task_id=request.goal_id;request.trace.run_id=request.goal_id;request.trace.subtask_id="navigate";
@@ -35,7 +35,7 @@ int main(int argc,char** argv) {
rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
auto record=registry.find(active_id);
if(!record) throw std::runtime_error("registry lost active goal: "+active_id);
if((scenario==1||scenario==6)&&record->accepted&&!canceled) {
if((scenario==1||scenario==6||scenario==9)&&record->accepted&&!canceled) {
registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true;
}
if(record->result) break;
@@ -51,7 +51,7 @@ int main(int argc,char** argv) {
if(!record||!record->result) throw std::runtime_error("result timeout");
const auto& result=*record->result;
bool blocked_redispatch=false;
if(scenario==6) {
if(scenario==6||scenario==9||scenario==10) {
auto next=request;next.goal_id+="-forbidden-retry";
next.trace.subtask_id="navigate-forbidden-retry";++next.trace.attempt;
blocked_redispatch=!registry.start(next,robot_bt::SteadyClock::now()).has_value();