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);
};