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

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_executor/include/bt_executor/ros_driver.hpp
+8 -7
View File
@@ -98,6 +98,7 @@ class RosDriver final : public robot_bt::GoalDriver {
bool fresh(robot_bt::RosTime observed, robot_bt::RosTime valid_until,
robot_bt::RosTime capture_after = 0) const;
robot_bt::ExecutionResult execution(const iface::msg::ExecutionResult&,const robot_bt::GoalRequest&) const;
robot_bt::ExecutionResult execution(const iface::msg::NavigationResult&,const robot_bt::GoalRequest&) const;
robot_bt::ExecutionResult readonly_result(rclcpp_action::ResultCode, bool valid,
robot_bt::SkillResponse) const;
robot_bt::SnapshotMeta meta(const robot_bt::GoalRequest&, robot_bt::RosTime,
@@ -137,13 +138,13 @@ class RosDriver final : public robot_bt::GoalDriver {
const std::shared_ptr<const typename Action::Feedback> feedback) {
if (!handle || !feedback || feedback->sequence == 0 || feedback->phase > max_phase) return;
if constexpr (std::is_same_v<Action, Navigate>) {
if(feedback->errors_valid&&(!std::isfinite(feedback->position_error)||feedback->position_error<0||
!std::isfinite(feedback->orientation_error)||std::abs(feedback->orientation_error)>std::acos(-1.0)))return;
if(feedback->pose_valid) {
if(feedback->error_valid&&(!std::isfinite(feedback->position_error)||feedback->position_error<0||
!std::isfinite(feedback->yaw_error)||std::abs(feedback->yaw_error)>std::acos(-1.0)))return;
if(feedback->current_pose_valid) {
const auto& p=feedback->current_pose;
robot_bt::Pose pose{p.header.frame_id,p.pose.position.x,p.pose.position.y,p.pose.position.z,
p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w};
if(!robot_bt::valid_pose(pose))return;
if(pose.frame_id!="map"||!robot_bt::valid_pose(pose))return;
}
}
if constexpr (std::is_same_v<Action, Manipulate>) {
@@ -168,9 +169,9 @@ class RosDriver final : public robot_bt::GoalDriver {
payload["progress_valid"]=feedback->progress_valid;payload["progress"]=feedback->progress;
}
if constexpr (std::is_same_v<Action, Navigate>) {
payload["pose_valid"]=feedback->pose_valid;payload["errors_valid"]=feedback->errors_valid;
payload["position_error"]=feedback->position_error;payload["orientation_error"]=feedback->orientation_error;
payload["blocked_valid"]=feedback->blocked_valid;payload["blocked"]=feedback->blocked;
payload["current_pose_valid"]=feedback->current_pose_valid;payload["error_valid"]=feedback->error_valid;
payload["position_error"]=feedback->position_error;payload["yaw_error"]=feedback->yaw_error;
payload["blocked"]=feedback->blocked;
}
event.feedback_snapshot=payload.dump();events_.push_back(event);
};
ros2/bt_executor/src/ros_driver.cpp
+25 -7
View File
@@ -170,6 +170,24 @@ ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const
r.stop=StopState::CONFIRMED;
return r;
}
ExecutionResult RosDriver::execution(const iface::msg::NavigationResult& m,const GoalRequest& request)const {
// Navigation outcomes have different numeric values from other skill results.
iface::msg::ExecutionResult common;
common.error_code=m.error_code;common.message=m.message;
common.stop_state=m.stop_state;common.stopped_at=m.stopped_at;common.stop_evidence_ref=m.stop_evidence_ref;
using Nav=iface::msg::NavigationResult;
using Common=iface::msg::ExecutionResult;
switch(m.status) {
case Nav::SUCCEEDED:common.status=Common::COMPLETED;break;
case Nav::CANCELED:common.status=Common::CANCELED;break;
case Nav::TIMEOUT:common.status=Common::TIMED_OUT;break;
case Nav::BLOCKED:common.status=Common::FAILED;if(common.error_code.empty())common.error_code="NAV_BLOCKED";break;
case Nav::NOT_READY:common.status=Common::REJECTED;if(common.error_code.empty())common.error_code="NAV_NOT_READY";break;
case Nav::FAILED:common.status=Common::FAILED;break;
default:{ExecutionResult invalid;invalid.error_code="NAV_RESULT_PROTOCOL_ERROR";return invalid;}
}
return execution(common,request);
}
ExecutionResult RosDriver::readonly_result(rclcpp_action::ResultCode native_code,bool valid,SkillResponse response)const {
ExecutionResult r;r.response=std::move(response);r.response.valid=valid;
// These servers are contractually read-only. Their native terminal is enough
@@ -210,13 +228,13 @@ void RosDriver::send(const GoalRequest& r) {
});break;
}
Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Navigate>(navigate_,g,r,6,[this,r](const Navigate::Result& m,auto){
auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&
std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&
m.final_position_error>=0&&std::abs(m.final_orientation_error)<=std::acos(-1.0)&&
m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);
g.position_tolerance=r.position_tolerance_m;g.yaw_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Navigate>(navigate_,g,r,Navigate::Feedback::STOPPING,[this,r](const Navigate::Result& m,auto){
auto out=execution(m.result,r);out.response.valid=m.final_pose_valid&&
std::isfinite(m.final_position_error)&&std::isfinite(m.final_yaw_error)&&
m.final_position_error>=0&&std::abs(m.final_yaw_error)<=std::acos(-1.0)&&
m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_yaw_error)<=r.orientation_tolerance_rad;
if(m.final_pose_valid)out.response.final_pose=pose_core(m.final_pose);
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
}
case Skill::PICK:case Skill::PLACE: {
ros2/bt_executor/tools/native_navigation_probe.cpp
+69
View File
@@ -0,0 +1,69 @@
// Test-only live DDS probe. Build via tests/helpers/native_navigation_contract.py.
#include <bt_executor/ros_driver.hpp>
#include <filesystem>
#include <iostream>
#include <thread>
int main(int argc,char** argv) {
if(argc!=4) return 2;
const int scenario=std::stoi(argv[1]);
const std::string journal=argv[2], ns=argv[3];
if(ns.rfind("/sim/",0)!=0) return 3;
rclcpp::init(0,nullptr);
try {
auto node=std::make_shared<rclcpp::Node>("native_navigation_probe",ns);
bt_executor::RosDriver driver(*node,"robot_01",journal+"/uuids.jsonl",0.5,10000000000LL,robot_bt::Milliseconds(5000));
robot_bt::TaskConfig task; task.route="LEGACY";
driver.bind_task(task,robot_bt::SiteConfig{},1);
robot_bt::ActiveGoalRegistry registry(driver,journal+"/goals.jsonl");
auto until=robot_bt::SteadyClock::now()+std::chrono::seconds(12);
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");
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";
request.skill=robot_bt::Skill::NAVIGATE;request.registered_pose=robot_bt::Pose{"map",double(scenario),0,0,0,0,0,1};
request.capture_after=node->now().nanoseconds();
const auto started=registry.start(request,robot_bt::SteadyClock::now());
if(!started) throw std::runtime_error("start refused");
const std::string active_id=*started;
bool canceled=false;
until=robot_bt::SteadyClock::now()+std::chrono::seconds(12);
while(robot_bt::SteadyClock::now()<until) {
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) {
registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true;
}
if(record->result) break;
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
// Pump beyond terminal delivery to expose accidental sends/retries in live transport.
until=robot_bt::SteadyClock::now()+std::chrono::milliseconds(350);
while(robot_bt::SteadyClock::now()<until) {
rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
auto record=registry.find(active_id);
if(!record||!record->result) throw std::runtime_error("result timeout");
const auto& result=*record->result;
bool blocked_redispatch=false;
if(scenario==6) {
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();
}
nlohmann::json report={{"scenario",scenario},{"code",int(result.code)},{"stop",int(result.stop)},
{"state",int(record->state)},{"robot_locked",registry.robot_locked("robot_01")},
{"error_code",result.error_code},{"detail",result.detail},{"feedback_sequence",record->last_sequence},
{"feedback",record->feedback_snapshot},{"wire_request_type",record->request.wire_request_type},
{"wire_result_type",result.wire_result_type},{"wire_result_bytes",result.wire_result_snapshot.size()/2},
{"response_valid",result.response.valid},{"mapping_count",driver.mappings().size()},
{"unknown_stop_blocks_redispatch",blocked_redispatch}};
std::cout<<report.dump()<<std::endl;
rclcpp::shutdown();return 0;
}catch(const std::exception& e){std::cerr<<e.what()<<std::endl;rclcpp::shutdown();return 1;}
}