实现行为树执行器、任务协调和技能接口

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -0,0 +1,32 @@
+#include "robot_bt/core.hpp"
+#include "robot_bt/sim_driver.hpp"
+#include <iostream>
+#include <map>
+#include <stdexcept>
+#include <string>
+using namespace robot_bt;
+namespace {
+std::string json(const std::string& text) {std::string result="\"";static const char hex[]="0123456789abcdef";for(unsigned char c:text){if(c=='"'||c=='\\'){result+='\\';result+=static_cast<char>(c);}else if(c<32){result+="\\u00";result+=hex[c>>4];result+=hex[c&15];}else result+=static_cast<char>(c);}return result+'"';}
+const char* status_name(TickStatus s){switch(s){case TickStatus::SUCCESS:return "SUCCEEDED";case TickStatus::FAILURE:return "FAILED";case TickStatus::INTERVENTION_REQUIRED:return "INTERVENTION_REQUIRED";default:return "EXECUTING";}}
+}
+int main(int argc,char** argv) {
+ bool jsonl=false;std::map<std::string,std::string> args={{"--target-ref","sim-item"},{"--destination-ref","sim-bin"},{"--task-id","sim-task"},{"--run-id","sim-run"},{"--robot-id","sim-robot"},{"--scenario","happy"},{"--route","LEGACY"},{"--item-index","0"}};
+ try {
+  for(int i=1;i<argc;++i){std::string key=argv[i];if(key=="--events-jsonl"){jsonl=true;continue;}if((key!="--journal"&&args.count(key)==0)||i+1>=argc)throw std::invalid_argument("usage: robot_bt_demo --journal PATH [--events-jsonl --target-ref ID --destination-ref ID --task-id ID --run-id ID --robot-id ID --scenario NAME]");args[key]=argv[++i];}
+  if(args["--journal"].empty())throw std::invalid_argument("explicit --journal PATH required");
+  SimDriver driver(args["--scenario"]);ActiveGoalRegistry registry(driver,args["--journal"]);ContextStore context;
+  TaskConfig task;task.trace={args["--task-id"],"fixed-template",args["--run-id"],1,1,1,1};task.robot_id=args["--robot-id"];task.target_id=args["--target-ref"];task.source_shelf="sim-shelf";task.destination_id=args["--destination-ref"];task.observe_location="sim-observe";task.destination_location="sim-destination";
+  task.route=args["--route"];task.item_index=static_cast<unsigned>(std::stoul(args["--item-index"]));
+  SiteConfig site;site.locations={{"sim-observe",Pose{"map",0,0,0,0,0,0,1}},{"sim-source",Pose{"map",1,0,0,0,0,0,1}},{"sim-destination",Pose{"map",2,0,0,0,0,0,1}}};site.parking_locations={{"sim-shelf/front/1","sim-source"}};site.allowed_postures={"registered-small-lift","registered-carry"};site.transport_posture="registered-carry";
+  site.object_locations[task.target_id]="sim-source";site.object_postures[task.target_id]="registered-small-lift";site.cell_locations["sim-shelf/front/1/2"]="sim-source";site.cell_postures["sim-shelf/front/1/2"]="registered-small-lift";
+  std::string evidence_id;bool delivered=false;
+  StageRunner runner(task,site,driver,registry,context,[&](const std::string& id,unsigned item,const std::string& evidence){if(id!=task.trace.task_id||item!=task.item_index||evidence.empty())return false;delivered=true;evidence_id=evidence;return true;});Workflow workflow(runner);
+  auto status=TickStatus::RUNNING;
+  for(unsigned tick=0;tick<500&&status==TickStatus::RUNNING;++tick){const auto ros=1000000+static_cast<RosTime>(tick)*50000000;driver.set_ros_time(ros);runner.update_safety({true,true,driver.sensor_holding(),ros,ros+1000000000});auto stage=workflow.current_stage();status=workflow.tick(SteadyTime{}+Milliseconds(tick*50),ros);if(workflow.current_stage()!=stage||status!=TickStatus::RUNNING){if(jsonl)std::cout<<"{\"type\":\"stage\",\"stage\":"<<json(stage_name(stage))<<",\"status\":"<<json(status_name(status))<<"}\n";else std::cout<<stage_name(stage)<<": "<<status_name(status)<<'\n';}}
+  if(status==TickStatus::RUNNING){workflow.halt(SteadyTime{}+Milliseconds(25000));status=TickStatus::INTERVENTION_REQUIRED;}
+  const bool success=status==TickStatus::SUCCESS&&delivered&&runner.holding()==Holding::EMPTY;
+  if(jsonl)std::cout<<"{\"type\":\"result\",\"status\":"<<json(status_name(status))<<",\"completed_quantity\":"<<(success?1:0)<<",\"stop_confirmed\":"<<(!registry.has_unresolved()?"true":"false")<<",\"simulated\":true,\"detail\":"<<json(runner.detail())<<",\"evidence\":{\"target_ref\":"<<json(task.target_id)<<",\"destination_ref\":"<<json(task.destination_id)<<",\"empty_hand\":"<<(success?"true":"false")<<",\"in_destination\":"<<(success?"true":"false")<<",\"safe_to_release\":"<<(success?"true":"false")<<",\"valid\":"<<(success?"true":"false")<<",\"passed\":"<<(success?"true":"false")<<",\"evidence_id\":"<<json(evidence_id)<<",\"source\":\"simulated-independent-verifier\"}}\n";
+  else std::cout<<"Simulation result: "<<status_name(status)<<"; delivered="<<(success?1:0)<<"; "<<runner.detail()<<'\n';
+  return success?0:1;
+ }catch(const std::exception& error){if(jsonl)std::cout<<"{\"type\":\"result\",\"status\":\"INTERVENTION_REQUIRED\",\"completed_quantity\":0,\"stop_confirmed\":false,\"simulated\":true,\"detail\":"<<json(error.what())<<",\"evidence\":{\"safe_to_release\":false,\"valid\":false,\"passed\":false}}\n";else std::cerr<<error.what()<<'\n';return 2;}
+}