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

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

Browse Source
This commit is contained in:
wangfeiyu
2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_executor/include/bt_executor/admission.hpp
+120
View File
@@ -0,0 +1,120 @@
#pragma once
#include <bt_executor/ros_driver.hpp>
#include <set>
#include <fstream>
#include <algorithm>
namespace bt_executor {
inline void require(bool ok,const std::string& message) {if(!ok)throw std::invalid_argument(message);}
inline json strict_json(const std::string& raw) {
require(raw.size()<=262144,"JSON exceeds size bound");
std::map<int,std::set<std::string>> keys;
return json::parse(raw,[&keys](int depth,json::parse_event_t event,json& value) {
if(event==json::parse_event_t::object_start)keys[depth+1].clear();
if(event==json::parse_event_t::key && !keys[depth].insert(value.get<std::string>()).second)
throw std::invalid_argument("duplicate JSON key");
return true;
});
}
inline void fields(const json& j,const std::set<std::string>& exact) {
require(j.is_object(),"expected object");std::set<std::string> got;
for(auto it=j.begin();it!=j.end();++it)got.insert(it.key());
require(got==exact,"unknown or missing JSON fields");
}
inline std::string string_value(const json& j,const char* key,std::size_t max=200) {
require(j.contains(key)&&j.at(key).is_string(),std::string("missing string ")+key);
auto s=j.at(key).get<std::string>();require(!s.empty()&&s.size()<=max,std::string("invalid string ")+key);return s;
}
inline robot_bt::SiteConfig load_site(const json& data) {
robot_bt::SiteConfig site;
require(data.at("schema_version")==1,"site schema mismatch");
require(data.at("locations").is_object(),"site locations required");
for(auto it=data.at("locations").begin();it!=data.at("locations").end();++it) {
const auto& p=it.value();
robot_bt::Pose pose;pose.frame_id=string_value(p,"frame_id");
pose.x=p.at("x").get<double>();pose.y=p.at("y").get<double>();pose.z=p.value("z",0.0);
pose.qx=p.value("qx",0.0);pose.qy=p.value("qy",0.0);pose.qz=p.value("qz",0.0);pose.qw=p.value("qw",1.0);
require(robot_bt::valid_pose(pose),"invalid registered pose");site.locations.emplace(it.key(),pose);
}
site.parking_locations=data.at("parking_locations").get<std::map<std::string,std::string>>();
site.allowed_postures=data.at("allowed_postures").get<std::vector<std::string>>();
site.transport_posture=string_value(data,"transport_posture");
require(std::find(site.allowed_postures.begin(),site.allowed_postures.end(),site.transport_posture)!=site.allowed_postures.end(),"transport posture not allowed");
for(const auto& kv:site.parking_locations)require(site.locations.count(kv.second),"unknown registered parking location");
for(const auto& name:{"object_locations","object_postures","cell_locations","cell_postures"})if(data.contains(name))require(data.at(name).is_object(),"lookup table must be object");
site.object_locations=data.value("object_locations",std::map<std::string,std::string>{});
site.object_postures=data.value("object_postures",std::map<std::string,std::string>{});
site.cell_locations=data.value("cell_locations",std::map<std::string,std::string>{});
site.cell_postures=data.value("cell_postures",std::map<std::string,std::string>{});
for(const auto* table:{&site.object_locations,&site.cell_locations})for(const auto& kv:*table)require(site.locations.count(kv.second),"unknown lookup location");
for(const auto* table:{&site.object_postures,&site.cell_postures})for(const auto& kv:*table)require(std::find(site.allowed_postures.begin(),site.allowed_postures.end(),kv.second)!=site.allowed_postures.end(),"unknown lookup posture");
return site;
}
inline robot_bt::TaskConfig admit_legacy(const json& p,const json& c,const iface::msg::TaskTrace& trace,
const json& trusted,const std::string& robot_id) {
fields(p,{"schema_version","plan_version","task_type","goal","slots","missing_information","subtasks"});
require(p.at("schema_version").is_number_integer()&&p.at("schema_version")==1,"plan schema mismatch");
require(p.at("plan_version").is_number_integer()&&p.at("plan_version")==trace.plan_version&&trace.plan_version>0,"plan version mismatch");
require(p.at("task_type")=="pick_transport_place","unsupported task type");string_value(p,"goal",4000);
require(p.at("missing_information").is_array()&&p.at("missing_information").empty(),"unresolved information");
const auto& slots=p.at("slots");fields(slots,{"target_name","quantity","source_location","destination"});
require(slots.at("quantity").is_number_integer()&&slots.at("quantity")==1,"quantity must be one");
const auto target=string_value(slots,"target_name"),source=string_value(slots,"source_location"),dest=string_value(slots,"destination");
const auto& subtasks=p.at("subtasks");require(subtasks.is_array()&&subtasks.size()==6,"fixed six-step chain required");
const std::vector<std::string> skills={"NAVIGATE","GROUND_TARGET","PICK","NAVIGATE","CHECK_FREE_SPACE","PLACE"};
const std::vector<json> args={{{"destination",source}},{{"target",target}},{{"target",target}},
{{"destination",dest}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
std::set<std::string> ids;
for(std::size_t i=0;i<subtasks.size();++i) {
const auto& s=subtasks[i];fields(s,{"id","skill","arguments","depends_on"});
auto id=string_value(s,"id",80);require(ids.insert(id).second,"duplicate subtask id");
require(s.at("skill")==skills.at(i)&&s.at("arguments")==args.at(i),"unsupported skill/arguments");
json deps=json::array();if(i)deps.push_back(subtasks[i-1].at("id"));
require(s.at("depends_on")==deps,"unsupported dependencies or cycle");
}
fields(c,{"schema_version","registry_version","robot_id","target_id","source_shelf","destination_id",
"observe_location","destination_location","task_revision"});
require(c.at("schema_version")==1&&c.at("registry_version")==trusted.at("registry_version"),"site registry mismatch");
require(c.at("robot_id")==robot_id&&c.at("task_revision")==trace.task_revision,"robot/revision mismatch");
require(c.at("target_id")==target,"plan/context target mismatch");
require(!trace.task_id.empty()&&!trace.run_id.empty()&&trace.task_revision>0&&trace.execution_generation>0&&trace.attempt>0,"invalid trace");
require(trusted.at("sources").contains(source)&&trusted.at("destinations").contains(dest),"unregistered site binding");
require(c.at("source_shelf")==trusted.at("sources").at(source).at("shelf_id")&&
c.at("destination_id")==trusted.at("destinations").at(dest).at("region_ref"),"plan/context binding mismatch");
require(c.at("observe_location")==trusted.at("sources").at(source).at("observe_location")&&
c.at("destination_location")==trusted.at("destinations").at(dest).at("location"),"registered station mismatch");
robot_bt::TaskConfig task;task.trace=trace_core(trace);task.robot_id=robot_id;task.target_id=target;
task.source_shelf=string_value(c,"source_shelf");task.destination_id=string_value(c,"destination_id");task.observe_location=string_value(c,"observe_location");
task.destination_location=string_value(c,"destination_location");return task;
}
inline robot_bt::TaskConfig admit(const json& p,const json& c,const iface::msg::TaskTrace& trace,const json& trusted,const std::string& robot_id) {
if(p.value("schema_version",0)==1)return admit_legacy(p,c,trace,trusted,robot_id);
fields(p,{"schema_version","plan_version","task_type","goal","route","slots","missing_information","subtasks"});
require(p.at("schema_version").is_number_integer()&&p.at("schema_version")==2,"unsupported schema");
const auto route=string_value(p,"route");require((route=="OBJECT_TABLE"||route=="SHELF_CELL")&&trusted.at("execution_route")==route,"route policy mismatch");
fields(p.at("slots"),{"items","destination"});const auto& items=p.at("slots").at("items");
require(items.is_array()&&items.size()==1,"executor accepts one physical item per run");
auto slots=items[0];fields(slots,{"target_name","quantity","source_location"});
slots["destination"]=p.at("slots").at("destination");
const auto target=string_value(slots,"target_name"),source=string_value(slots,"source_location"),dest=string_value(slots,"destination");
std::vector<std::string> skills={"NAVIGATE","PICK","NAVIGATE","PLACE"};
std::vector<json> args={{{"target",target}},{{"target",target}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
if(route=="SHELF_CELL") {
skills={"NAVIGATE","ROBOBRAIN_SHELF_LOCALIZE","NAVIGATE","PICK","NAVIGATE","PLACE"};
args={{{"source_location",source},{"mode","observation"}},{{"target",target}},{{"source_location",source},{"mode","shelf_cell"}},{{"target",target}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
}
const auto& steps=p.at("subtasks");require(steps.is_array()&&steps.size()==skills.size(),"invalid fixed v2 steps");std::set<std::string> ids;
for(std::size_t i=0;i<steps.size();++i){
const auto& step=steps[i];fields(step,{"id","skill","arguments","depends_on"});auto id=string_value(step,"id",80);require(ids.insert(id).second,"duplicate ID");
args[i]["item_index"]=0;require(step.at("skill")==skills[i]&&step.at("arguments")==args[i],"invalid fixed v2 arguments");
require(step.at("arguments").at("item_index").is_number_integer(),"item index type");
auto deps=json::array();if(i)deps.push_back(steps[i-1].at("id"));require(step.at("depends_on")==deps,"invalid v2 dependencies");
}
require(c.at("route")==route&&c.at("item_index").is_number_unsigned()&&c.at("item_index").get<unsigned>()<20,"invalid item context");
auto clean=c;clean.erase("route");clean.erase("item_index");auto legacy=p;legacy.erase("route");legacy["schema_version"]=1;legacy["slots"]=slots;
const std::vector<std::string> old_skills={"NAVIGATE","GROUND_TARGET","PICK","NAVIGATE","CHECK_FREE_SPACE","PLACE"};
const std::vector<json> old_args={{{"destination",source}},{{"target",target}},{{"target",target}},{{"destination",dest}},{{"destination",dest}},{{"target",target},{"destination",dest}}};
legacy["subtasks"]=json::array();for(unsigned i=0;i<6;++i){auto deps=json::array();if(i)deps.push_back("S"+std::to_string(i));legacy["subtasks"].push_back({{"id","S"+std::to_string(i+1)},{"skill",old_skills[i]},{"arguments",old_args[i]},{"depends_on",deps}});}
auto task=admit_legacy(legacy,clean,trace,trusted,robot_id);task.route=route;task.item_index=c.at("item_index").get<unsigned>();return task;
}
} // namespace bt_executor
ros2/bt_executor/include/bt_executor/ros_driver.hpp
+172
View File
@@ -0,0 +1,172 @@
#pragma once
#include <robot_bt/core.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <bt_skill_interfaces/action/navigate.hpp>
#include <bt_skill_interfaces/action/navigate_semantic.hpp>
#include <bt_skill_interfaces/action/execute_manipulation.hpp>
#include <bt_skill_interfaces/action/locate_shelf_column.hpp>
#include <bt_skill_interfaces/action/localize_target3_d.hpp>
#include <bt_skill_interfaces/action/assess_grasp.hpp>
#include <bt_skill_interfaces/action/execute_posture.hpp>
#include <bt_skill_interfaces/action/verify_state.hpp>
#include <bt_skill_interfaces/action/check_free_space.hpp>
#include <bt_skill_interfaces/msg/safety_state.hpp>
#include <bt_skill_interfaces/msg/robot_state.hpp>
#include <bt_skill_interfaces/msg/target_binding.hpp>
#include <bt_skill_interfaces/msg/placement_binding.hpp>
#include <nlohmann/json.hpp>
#include <functional>
#include <memory>
#include <set>
#include <type_traits>
#include <cmath>
namespace bt_executor {
namespace iface = bt_skill_interfaces;
using json = nlohmann::json;
robot_bt::RosTime ns(const builtin_interfaces::msg::Time&);
builtin_interfaces::msg::Time stamp(robot_bt::RosTime);
iface::msg::TaskTrace trace_msg(const robot_bt::Trace&);
robot_bt::Trace trace_core(const iface::msg::TaskTrace&);
robot_bt::NativeStatus native(rclcpp_action::ResultCode);
// All methods and callbacks run on the node's single-threaded executor. Callbacks
// enqueue immutable request-bound events; only ActiveGoalRegistry advances state.
class RosDriver final : public robot_bt::GoalDriver {
public:
RosDriver(rclcpp::Node&, std::string robot_id, std::string uuid_journal,
double min_confidence, std::int64_t observation_lifetime_ns,
robot_bt::Milliseconds skill_timeout);
bool ready(robot_bt::Skill) const override;
void send(const robot_bt::GoalRequest&) override;
void cancel(const std::string&) override;
std::vector<robot_bt::GoalEvent> drain_events() override;
robot_bt::SafetySnapshot safety(const std::string& target_id) const;
json mappings() const;
bool faulted() const { return faulted_; }
void bind_task(const robot_bt::TaskConfig& task,const robot_bt::SiteConfig& site,std::uint32_t registry_version) {
current_task_=task;allowed_postures_=site.allowed_postures;registry_version_=registry_version;
}
std::optional<std::uint64_t> geometry_epoch() const;
private:
using Semantic = iface::action::NavigateSemantic;
using Navigate = iface::action::Navigate;
using Manipulate = iface::action::ExecuteManipulation;
using Locate = iface::action::LocateShelfColumn;
using Localize = iface::action::LocalizeTarget3D;
using Assess = iface::action::AssessGrasp;
using Posture = iface::action::ExecutePosture;
using Verify = iface::action::VerifyState;
using Space = iface::action::CheckFreeSpace;
rclcpp::Node& node_;
std::string robot_id_, uuid_journal_;
double min_confidence_;
std::int64_t observation_lifetime_ns_;
builtin_interfaces::msg::Duration skill_timeout_;
bool faulted_{false};
rclcpp_action::Client<Navigate>::SharedPtr navigate_;
rclcpp_action::Client<Semantic>::SharedPtr semantic_;
rclcpp_action::Client<Manipulate>::SharedPtr manipulate_;
rclcpp_action::Client<Locate>::SharedPtr locate_;
rclcpp_action::Client<Localize>::SharedPtr localize_;
rclcpp_action::Client<Assess>::SharedPtr assess_;
rclcpp_action::Client<Posture>::SharedPtr posture_;
rclcpp_action::Client<Verify>::SharedPtr verify_;
rclcpp_action::Client<Space>::SharedPtr space_;
rclcpp::Subscription<iface::msg::SafetyState>::SharedPtr safety_sub_;
rclcpp::Subscription<iface::msg::RobotState>::SharedPtr robot_sub_;
std::optional<iface::msg::SafetyState> safety_state_;
std::optional<iface::msg::RobotState> robot_state_;
std::vector<robot_bt::GoalEvent> events_;
std::map<std::string, std::function<void()>> cancelers_;
std::set<std::string> cancel_intents_;
std::map<std::string, json> mappings_;
std::map<std::string, json> shelf_bindings_;
robot_bt::TaskConfig current_task_;
std::vector<std::string> allowed_postures_;
std::uint32_t registry_version_{0};
rclcpp::Publisher<iface::msg::TargetBinding>::SharedPtr target_pub_;
rclcpp::Publisher<iface::msg::PlacementBinding>::SharedPtr placement_pub_;
void record_mapping(const robot_bt::GoalRequest&, const rclcpp_action::GoalUUID&);
builtin_interfaces::msg::Duration timeout() const { return skill_timeout_; }
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 readonly_result(rclcpp_action::ResultCode, bool valid,
robot_bt::SkillResponse) const;
robot_bt::SnapshotMeta meta(const robot_bt::GoalRequest&, robot_bt::RosTime,
robot_bt::RosTime, const std::string&) const;
template <class Action, class Decode>
void send_typed(const typename rclcpp_action::Client<Action>::SharedPtr& client,
typename Action::Goal goal, const robot_bt::GoalRequest& request,
unsigned max_phase, Decode decode) {
using Handle = rclcpp_action::ClientGoalHandle<Action>;
typename rclcpp_action::Client<Action>::SendGoalOptions options;
options.goal_response_callback = [this, client, request](typename Handle::SharedPtr handle) {
robot_bt::GoalEvent event;
event.goal_id = request.goal_id; event.trace = request.trace;
if (!handle) { event.kind = robot_bt::EventKind::REJECTED; events_.push_back(event); return; }
// Humble assigns the wire UUID inside async_send_goal; the persisted client
// goal_id already exists before send. Record the wire mapping on acceptance.
try { record_mapping(request, handle->get_goal_id()); }
catch (const std::exception& e) {
faulted_ = true; cancel_intents_.insert(request.goal_id);
RCLCPP_ERROR(node_.get_logger(), "UUID journal failure: %s", e.what());
}
cancelers_[request.goal_id] = [this, client, handle, request] {
client->async_cancel_goal(handle, [this, request](auto response) {
if (!response || response->return_code != 0) return;
robot_bt::GoalEvent ack; ack.kind = robot_bt::EventKind::CANCEL_ACK;
ack.goal_id = request.goal_id; ack.trace = request.trace; events_.push_back(ack);
});
};
event.kind = robot_bt::EventKind::ACCEPTED; events_.push_back(event);
if (cancel_intents_.count(request.goal_id)) cancelers_.at(request.goal_id)();
};
options.feedback_callback = [this, request, max_phase](typename Handle::SharedPtr handle,
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) {
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 constexpr (std::is_same_v<Action, Manipulate>) {
if(feedback->progress_valid&&(!std::isfinite(feedback->progress)||feedback->progress<0||feedback->progress>1))return;
}
if constexpr (std::is_same_v<Action, Navigate>||std::is_same_v<Action, Manipulate>) {
if(feedback->elapsed_time.sec<0||feedback->elapsed_time.nanosec>=1000000000)return;
}
const auto at = ns(feedback->stamp), now = node_.now().nanoseconds();
// Feedback from the future, stale feedback, and malformed phases do not
// refresh liveness. Phase itself may legitimately move backwards.
if (at <= 0 || at > now || now - at > observation_lifetime_ns_) return;
robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::FEEDBACK;
event.goal_id = request.goal_id; event.trace = request.trace;
event.sequence = feedback->sequence; events_.push_back(event);
};
options.result_callback = [this, request, decode](const typename Handle::WrappedResult& result) {
robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::RESULT;
event.goal_id = request.goal_id; event.trace = request.trace;
event.native_status = native(result.code);
if (result.result) {
try { event.result = decode(*result.result, result.code); }
catch (const std::exception& e) { event.result.detail = std::string("invalid result: ") + e.what(); }
}
events_.push_back(event);
cancelers_.erase(request.goal_id);
cancel_intents_.erase(request.goal_id);
};
// No spin_until_future_complete, wait_for_action_server or blocking get here.
(void)client->async_send_goal(goal, options);
}
};
} // namespace bt_executor