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

Unify navigation on NavigateToPose and remove legacy proxies

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 17:40:25 +08:00
parent 964d1fde67
commit 24e0b922bc
40 changed files with 461 additions and 2592 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_executor/include/bt_executor/ros_driver.hpp
+5 -7
View File
@@ -3,8 +3,7 @@
#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>
#include <navigation_interfaces/action/navigate_to_pose.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>
@@ -54,8 +53,7 @@ class RosDriver final : public robot_bt::GoalDriver {
std::optional<std::uint64_t> geometry_epoch() const;
private:
using Semantic = iface::action::NavigateSemantic;
using Navigate = iface::action::Navigate;
using Navigate = navigation_interfaces::action::NavigateToPose;
using Manipulate = iface::action::ExecuteManipulation;
using Locate = iface::action::LocateShelfColumn;
using Localize = iface::action::LocalizeTarget3D;
@@ -70,7 +68,6 @@ class RosDriver final : public robot_bt::GoalDriver {
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_;
@@ -98,7 +95,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 execution(const Navigate::Result&,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,
@@ -171,7 +168,8 @@ class RosDriver final : public robot_bt::GoalDriver {
if constexpr (std::is_same_v<Action, Navigate>) {
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;
payload["blocked_valid"]=feedback->blocked_valid;
payload["blocked"]=feedback->blocked_valid?json(feedback->blocked):json(nullptr);
}
event.feedback_snapshot=payload.dump();events_.push_back(event);
};