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

fix: harden task recovery and DR contract handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-20 13:36:48 +08:00
parent 492676344a
commit f9d8feb6f0
49 changed files with 2083 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_executor/src/ros_driver.cpp
+24 -9
View File
@@ -6,6 +6,7 @@
#include <cmath>
#include <iomanip>
#include <fstream>
#include <filesystem>
#include <sstream>
#include <stdexcept>
@@ -83,6 +84,7 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
auto value=json::parse(line);const auto id=value.at("client_goal_id").get<std::string>();
if(id.empty()||value.at("ros_goal_uuid").get<std::string>().size()!=32)throw std::runtime_error("corrupt UUID journal");
mappings_[id]=std::move(value);
prune_mappings();
}
navigate_=rclcpp_action::create_client<Navigate>(&n,n.declare_parameter<std::string>("navigate_action","skills/navigate"));
manipulate_=rclcpp_action::create_client<Manipulate>(&n,n.declare_parameter<std::string>("execute_manipulation_action","skills/execute_manipulation"));
@@ -139,15 +141,27 @@ void RosDriver::record_mapping(const GoalRequest& r,const rclcpp_action::GoalUUI
json m={{"client_goal_id",r.goal_id},{"ros_goal_uuid",hex.str()},{"task_id",r.trace.task_id},
{"run_id",r.trace.run_id},{"task_revision",r.trace.task_revision},{"plan_version",r.trace.plan_version},
{"execution_generation",r.trace.execution_generation},{"subtask_id",r.trace.subtask_id},{"attempt",r.trace.attempt}};
durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m;
durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m;prune_mappings(r.goal_id);
}
json RosDriver::mappings()const {json j=json::array();for(const auto& kv:mappings_)j.push_back(kv.second);return j;}
void RosDriver::prune_mappings(const std::string& keep) {
for(auto it=mappings_.begin();mappings_.size()>256&&it!=mappings_.end();) {
if(it->first!=keep&&!cancelers_.count(it->first)&&!cancel_intents_.count(it->first))it=mappings_.erase(it);else ++it;
}
}
json RosDriver::mappings(const std::string& run_id)const {
std::ifstream input(uuid_journal_);if(!input&&std::filesystem::exists(uuid_journal_))throw std::runtime_error("UUID journal unreadable");
std::map<std::string,json> selected;std::string line;
while(std::getline(input,line)){if(line.empty())continue;auto entry=json::parse(line);if(entry.at("run_id")==run_id){const auto id=entry.at("client_goal_id").get<std::string>();selected[id]=std::move(entry);}}
if(input.bad())throw std::runtime_error("UUID journal read failed");
json out=json::array();for(const auto& item:selected)out.push_back(item.second);return out;
}
void RosDriver::cancel(const std::string& id) {
cancel_intents_.insert(id);auto it=cancelers_.find(id);if(it!=cancelers_.end())it->second();
}
std::vector<GoalEvent> RosDriver::drain_events(){std::vector<GoalEvent> v;v.swap(events_);return v;}
ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const GoalRequest& request)const {
ExecutionResult r;r.detail=m.error_code+": "+m.message;
ExecutionResult r;r.detail=m.error_code+": "+m.message;r.error_code=m.error_code;
switch(m.status){case 0:r.code=ResultCode::COMPLETED;break;case 1:r.code=ResultCode::FAILED;break;
case 2:r.code=ResultCode::CANCELED;break;case 3:r.code=ResultCode::TIMED_OUT;break;
case 4:r.code=ResultCode::REJECTED;break;default:return r;}
@@ -191,7 +205,8 @@ void RosDriver::send(const GoalRequest& r) {
Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Semantic>(semantic_,g,r,6,[this,r](const Semantic::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&&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);out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;
if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;
});break;
}
Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
@@ -210,7 +225,7 @@ void RosDriver::send(const GoalRequest& r) {
if(r.skill==Skill::PLACE){g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;}
g.timeout=timeout();
send_typed<Manipulate>(manipulate_,g,r,5,[this,r](const Manipulate::Result& m,auto){
auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty();
auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty();out.execution_record_ref=m.execution_record_ref;
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
}
case Skill::LOCATE_SHELF_COLUMN: {
@@ -223,7 +238,7 @@ void RosDriver::send(const GoalRequest& r) {
!m.observation_id.empty()&&!m.record_ref.empty()&&!m.shelf_id.empty()&&!m.side_id.empty()&&!m.column_id.empty()&&
fresh(ns(m.observed_at),ns(m.observed_at)+observation_lifetime_ns_,r.capture_after);
if(ok)shelf_bindings_[r.trace.run_id]={{"shelf",m.shelf_id},{"side",m.side_id},{"column",m.column_id},{"tier",m.tier_id},{"record",m.record_ref}};
return readonly_result(code,ok,out);});break;
auto result=readonly_result(code,ok,out);result.error_code=m.status==Locate::Result::NOT_FOUND?"NOT_FOUND":m.status==Locate::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});break;
}
case Skill::LOCALIZE_TARGET: {
Localize::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;
@@ -247,7 +262,7 @@ void RosDriver::send(const GoalRequest& r) {
msg.target.object_ref=m.target_ref;msg.target.description=r.target_id;msg.target_point=m.target_point;
msg.grasp_point=m.grasp_point;msg.grasp_point_valid=m.grasp_point_valid;msg.grasp_region_ref=m.grasp_region_ref;
msg.geometry_valid=true;msg.calibration_id=m.calibration_id;msg.shelf_id=r.shelf;target_pub_->publish(msg);}
return readonly_result(code,ok,out);});break;
auto result=readonly_result(code,ok,out);result.error_code=m.status==Localize::Result::NOT_FOUND?"NOT_FOUND":m.status==Localize::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});break;
}
case Skill::EVALUATE_GRASP: {
if(!r.target||!robot_state_)throw std::runtime_error("assess missing binding/state");
@@ -263,7 +278,7 @@ void RosDriver::send(const GoalRequest& r) {
switch(m.decision){case 0:out.admission=Admission::DIRECT;break;case 1:out.admission=Admission::ADJUST_POSTURE;break;
case 2:out.admission=Admission::NOT_REACHABLE;break;default:out.admission=Admission::UNKNOWN;}
bool ok=m.decision<=3&&m.geometry_epoch==r.geometry_epoch&&!m.evidence_ref.empty();
return readonly_result(code,ok,out);});break;
auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.evidence_ref;return result;});break;
}
case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE: {
Posture::Goal g;g.trace=trace_msg(r.trace);g.posture_id=r.posture_id;
@@ -299,7 +314,7 @@ void RosDriver::send(const GoalRequest& r) {
else if(r.skill==Skill::VERIFY_EMPTY)out.verified=out.verified&&e.hand_empty_valid&&e.hand_empty&&out.holding==Holding::EMPTY;
else out.verified=out.verified&&e.destination_ref==r.destination_id&&e.hand_empty_valid&&e.hand_empty&&
out.holding==Holding::EMPTY&&out.in_destination;
return readonly_result(code,ok&&e.status<=2,out);});break;
auto result=readonly_result(code,ok&&e.status<=2,out);result.error_code=e.error_code;result.detail=e.message;result.execution_record_ref=e.evidence_ref;return result;});break;
}
case Skill::CHECK_FREE_SPACE: {
Space::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.destination_ref=r.destination_id;
@@ -319,7 +334,7 @@ void RosDriver::send(const GoalRequest& r) {
msg.placement_region_ref=m.placement_region_ref;msg.placement_point=m.placement_point;
msg.placement_point_valid=m.placement_point_valid;msg.placement_pose=m.placement_pose;
msg.placement_pose_valid=m.placement_pose_valid;msg.geometry_valid=true;placement_pub_->publish(msg);}
return readonly_result(code,ok,out);});break;
auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.record_ref;return result;});break;
}
}
}