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/include/bt_executor/recovery_policy.hpp
+18
View File
@@ -0,0 +1,18 @@
#pragma once
#include <string>
#include <cstddef>
namespace bt_executor {
// Compare every byte in the configured secret without an early mismatch exit.
inline bool recovery_authorized(const std::string& expected,const std::string& supplied) {
if(expected.size()<16||expected.size()>1024||supplied.size()>1024)return false;
std::size_t difference=expected.size()^supplied.size();
for(std::size_t i=0;i<expected.size();++i)
difference|=static_cast<unsigned char>(expected[i])^
static_cast<unsigned char>(i<supplied.size()?supplied[i]:0);
return difference==0;
}
inline bool recovery_resume_permitted(bool complete_history,bool manipulation_dispatched,bool receipt) {
return receipt||(complete_history&&!manipulation_dispatched);
}
} // namespace bt_executor
ros2/bt_executor/include/bt_executor/ros_driver.hpp
+37 -1
View File
@@ -1,6 +1,7 @@
#pragma once
#include <robot_bt/core.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/serialization.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <bt_skill_interfaces/action/navigate.hpp>
#include <bt_skill_interfaces/action/navigate_semantic.hpp>
@@ -44,8 +45,10 @@ class RosDriver final : public robot_bt::GoalDriver {
std::vector<robot_bt::GoalEvent> drain_events() override;
robot_bt::SafetySnapshot safety(const std::string& target_id) const;
json mappings() const;
json mappings(const std::string& run_id) const;
bool faulted() const { return faulted_; }
void bind_task(const robot_bt::TaskConfig& task,const robot_bt::SiteConfig& site,std::uint32_t registry_version) {
shelf_bindings_.clear();
current_task_=task;allowed_postures_=site.allowed_postures;registry_version_=registry_version;
}
std::optional<std::uint64_t> geometry_epoch() const;
@@ -90,6 +93,7 @@ class RosDriver final : public robot_bt::GoalDriver {
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&);
void prune_mappings(const std::string& keep = {});
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;
@@ -104,6 +108,9 @@ class RosDriver final : public robot_bt::GoalDriver {
typename Action::Goal goal, const robot_bt::GoalRequest& request,
unsigned max_phase, Decode decode) {
using Handle = rclcpp_action::ClientGoalHandle<Action>;
// Capture the exact generated Goal after construction and before transport.
// The registry synchronously fsyncs this record; failure prevents sending.
record_wire_request(request.goal_id,rosidl_generator_traits::name<typename Action::Goal>(),serialized_hex(goal));
typename rclcpp_action::Client<Action>::SendGoalOptions options;
options.goal_response_callback = [this, client, request](typename Handle::SharedPtr handle) {
robot_bt::GoalEvent event;
@@ -151,7 +158,21 @@ class RosDriver final : public robot_bt::GoalDriver {
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);
event.sequence = feedback->sequence;
json payload={{"type",rosidl_generator_traits::name<typename Action::Feedback>()},{"cdr_hex",serialized_hex(*feedback)},
{"stamp_ns",at},{"sequence",feedback->sequence},{"phase",feedback->phase},{"message",feedback->message}};
if constexpr (std::is_same_v<Action, Navigate>||std::is_same_v<Action, Manipulate>) {
payload["elapsed_time_ns"]=std::int64_t(feedback->elapsed_time.sec)*1000000000LL+feedback->elapsed_time.nanosec;
}
if constexpr (std::is_same_v<Action, Manipulate>) {
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;
}
event.feedback_snapshot=payload.dump();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;
@@ -160,13 +181,28 @@ class RosDriver final : public robot_bt::GoalDriver {
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(); }
try {
event.result.wire_result_type=rosidl_generator_traits::name<typename Action::Result>();
event.result.wire_result_snapshot=serialized_hex(*result.result);
} catch(const std::exception& e) {
event.result.stop=robot_bt::StopState::UNKNOWN;event.result.response.valid=false;
event.result.detail=std::string("result snapshot unavailable: ")+e.what();
}
}
events_.push_back(event);
cancelers_.erase(request.goal_id);
cancel_intents_.erase(request.goal_id);
prune_mappings();
};
// No spin_until_future_complete, wait_for_action_server or blocking get here.
(void)client->async_send_goal(goal, options);
}
template<class Message> static std::string serialized_hex(const Message& message) {
rclcpp::Serialization<Message> serialization;rclcpp::SerializedMessage bytes;
serialization.serialize_message(&message,&bytes);
const auto& raw=bytes.get_rcl_serialized_message();static const char hex[]="0123456789abcdef";
std::string out;out.reserve(raw.buffer_length*2);
for(std::size_t i=0;i<raw.buffer_length;++i){out.push_back(hex[raw.buffer[i]>>4]);out.push_back(hex[raw.buffer[i]&15]);}return out;
}
};
} // namespace bt_executor