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/CMakeLists.txt
+2 -1
View File
@@ -7,6 +7,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(bt_skill_interfaces REQUIRED)
find_package(navigation_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(behaviortree_cpp 4.10.0 EXACT REQUIRED)
@@ -18,7 +19,7 @@ target_include_directories(robot_bt_core PUBLIC ../../core/include)
add_executable(bt_executor_node src/executor_node.cpp src/ros_driver.cpp)
target_include_directories(bt_executor_node PRIVATE include)
target_link_libraries(bt_executor_node robot_bt_core behaviortree_cpp::behaviortree_cpp nlohmann_json::nlohmann_json)
ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs)
ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces navigation_interfaces geometry_msgs std_msgs)
target_compile_options(bt_executor_node PRIVATE -Wall -Wextra -Wpedantic)
install(TARGETS bt_executor_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY trees launch config DESTINATION share/${PROJECT_NAME})
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);
};
ros2/bt_executor/package.xml
+1 -1
View File
@@ -5,7 +5,7 @@
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend><depend>rclcpp_action</depend><depend>ament_index_cpp</depend>
<depend>bt_skill_interfaces</depend><depend>geometry_msgs</depend><depend>std_msgs</depend>
<depend>bt_skill_interfaces</depend><depend>navigation_interfaces</depend><depend>geometry_msgs</depend><depend>std_msgs</depend>
<depend version_eq="4.10.0">behaviortree_cpp</depend><depend>nlohmann_json</depend>
<exec_depend>launch_ros</exec_depend><exec_depend>launch</exec_depend>
<export><build_type>ament_cmake</build_type></export>
ros2/bt_executor/src/ros_driver.cpp
+18 -27
View File
@@ -89,7 +89,6 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
navigate_=rclcpp_action::create_client<Navigate>(&n,n.declare_parameter<std::string>("navigate_action","skills/navigate"));
manipulate_=rclcpp_action::create_client<Manipulate>(&n,n.declare_parameter<std::string>("execute_manipulation_action","skills/execute_manipulation"));
locate_=rclcpp_action::create_client<Locate>(&n,n.declare_parameter<std::string>("locate_shelf_column_action","skills/locate_shelf_column"));
semantic_=rclcpp_action::create_client<Semantic>(&n,n.declare_parameter<std::string>("navigate_semantic_action","skills/navigate_semantic"));
localize_=rclcpp_action::create_client<Localize>(&n,n.declare_parameter<std::string>("localize_target_3d_action","skills/localize_target_3d"));
assess_=rclcpp_action::create_client<Assess>(&n,n.declare_parameter<std::string>("assess_grasp_action","skills/assess_grasp"));
posture_=rclcpp_action::create_client<Posture>(&n,n.declare_parameter<std::string>("execute_posture_action","skills/execute_posture"));
@@ -126,7 +125,7 @@ std::optional<std::uint64_t> RosDriver::geometry_epoch() const {
bool RosDriver::ready(Skill skill)const {
if(faulted_)return false;
switch(skill) {
case Skill::NAVIGATE:return current_task_.route=="LEGACY"?navigate_->action_server_is_ready():semantic_->action_server_is_ready();
case Skill::NAVIGATE:return navigate_->action_server_is_ready();
case Skill::PICK:case Skill::PLACE:return manipulate_->action_server_is_ready();
case Skill::LOCATE_SHELF_COLUMN:return locate_->action_server_is_ready();
case Skill::LOCALIZE_TARGET:return localize_->action_server_is_ready();
@@ -170,23 +169,23 @@ ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const
r.stop=StopState::CONFIRMED;
return r;
}
ExecutionResult RosDriver::execution(const iface::msg::NavigationResult& m,const GoalRequest& request)const {
// Navigation outcomes have different numeric values from other skill results.
iface::msg::ExecutionResult common;
common.error_code=m.error_code;common.message=m.message;
common.stop_state=m.stop_state;common.stopped_at=m.stopped_at;common.stop_evidence_ref=m.stop_evidence_ref;
using Nav=iface::msg::NavigationResult;
using Common=iface::msg::ExecutionResult;
ExecutionResult RosDriver::execution(const Navigate::Result& m,const GoalRequest& request)const {
ExecutionResult out;out.error_code=m.error_code;
using Nav=Navigate::Result;
switch(m.status) {
case Nav::SUCCEEDED:common.status=Common::COMPLETED;break;
case Nav::CANCELED:common.status=Common::CANCELED;break;
case Nav::TIMEOUT:common.status=Common::TIMED_OUT;break;
case Nav::BLOCKED:common.status=Common::FAILED;if(common.error_code.empty())common.error_code="NAV_BLOCKED";break;
case Nav::NOT_READY:common.status=Common::REJECTED;if(common.error_code.empty())common.error_code="NAV_NOT_READY";break;
case Nav::FAILED:common.status=Common::FAILED;break;
default:{ExecutionResult invalid;invalid.error_code="NAV_RESULT_PROTOCOL_ERROR";return invalid;}
case Nav::SUCCEEDED:out.code=ResultCode::COMPLETED;break;
case Nav::CANCELED:out.code=ResultCode::CANCELED;break;
case Nav::TIMEOUT:out.code=ResultCode::TIMED_OUT;break;
case Nav::BLOCKED:out.code=ResultCode::FAILED;if(out.error_code.empty())out.error_code="NAV_BLOCKED";break;
case Nav::NOT_READY:out.code=ResultCode::REJECTED;if(out.error_code.empty())out.error_code="NAV_NOT_READY";break;
case Nav::FAILED:out.code=ResultCode::FAILED;break;
default:out.error_code="NAV_RESULT_PROTOCOL_ERROR";return out;
}
return execution(common,request);
out.detail=out.error_code+": "+m.message;
if(m.stop_state==Nav::STOP_CONFIRMED&&!m.stop_evidence_ref.empty()&&
fresh(ns(m.stopped_at),ns(m.stopped_at)+observation_lifetime_ns_,request.capture_after))
out.stop=StopState::CONFIRMED;
return out;
}
ExecutionResult RosDriver::readonly_result(rclcpp_action::ResultCode native_code,bool valid,SkillResponse response)const {
ExecutionResult r;r.response=std::move(response);r.response.valid=valid;
@@ -219,18 +218,10 @@ void RosDriver::send(const GoalRequest& r) {
switch(r.skill) {
case Skill::NAVIGATE: {
if(!r.registered_pose)throw std::runtime_error("registered navigation pose required");
if(!r.navigation_kind.empty()) {
Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Semantic>(semantic_,g,r,6,[this,r](const Semantic::Result& m,auto){
auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&m.final_position_error>=0&&m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;
});break;
}
Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
Navigate::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.target_pose=pose_msg(*r.registered_pose,now);
g.position_tolerance=r.position_tolerance_m;g.yaw_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Navigate>(navigate_,g,r,Navigate::Feedback::STOPPING,[this,r](const Navigate::Result& m,auto){
auto out=execution(m.result,r);out.response.valid=m.final_pose_valid&&
auto out=execution(m,r);out.response.valid=m.final_pose_valid&&
std::isfinite(m.final_position_error)&&std::isfinite(m.final_yaw_error)&&
m.final_position_error>=0&&std::abs(m.final_yaw_error)<=std::acos(-1.0)&&
m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_yaw_error)<=r.orientation_tolerance_rad;
ros2/bt_executor/tools/build_humble.sh
+1 -1
View File
@@ -14,5 +14,5 @@ cd "$bt_repo_root"
# Install/build upstream BehaviorTree.CPP 4.10.0 separately and source its prefix.
# EXACT REQUIRED intentionally rejects an incompatible system BT.CPP package.
colcon build --base-paths ros2 --packages-up-to bt_executor robobrain_services bt_mock_servers --event-handlers console_direct+
colcon test --packages-select bt_skill_interfaces bt_executor --event-handlers console_direct+
colcon test --packages-select navigation_interfaces bt_skill_interfaces bt_executor --event-handlers console_direct+
colcon test-result --verbose
ros2/bt_executor/tools/check_static.py
+2 -2
View File
@@ -38,8 +38,8 @@ def header_name(name):
for source in list((PACKAGE / 'src').glob('*.cpp')) + list((PACKAGE / 'include/bt_executor').glob('*.hpp')):
for kind, name in re.findall(r'bt_skill_interfaces/(action|msg)/(\w+)\.hpp', source.read_text()):
candidates = list((PACKAGE.parent / 'bt_skill_interfaces' / kind).glob('*'))
for package, kind, name in re.findall(r'(bt_skill_interfaces|navigation_interfaces)/(action|msg)/(\w+)\.hpp', source.read_text()):
candidates = list((PACKAGE.parent / package / kind).glob('*'))
assert any(header_name(v.stem) == name for v in candidates), (source, kind, name)
ET.parse(PACKAGE / 'package.xml')
print(f'Static checks passed: {len(actual)} ordered core stages, six skill templates, generated IDL include names, package XML.')
ros2/bt_executor/tools/native_navigation_probe.cpp
+3 -3
View File
@@ -20,7 +20,7 @@ int main(int argc,char** argv) {
while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()<until) {
rclcpp::spin_some(node); std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("Navigate discovery timed out");
if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("NavigateToPose discovery timed out");
robot_bt::GoalRequest request;
request.goal_id="probe-"+std::to_string(scenario);request.robot_id="robot_01";
request.trace.task_id=request.goal_id;request.trace.run_id=request.goal_id;request.trace.subtask_id="navigate";
@@ -35,7 +35,7 @@ int main(int argc,char** argv) {
rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
auto record=registry.find(active_id);
if(!record) throw std::runtime_error("registry lost active goal: "+active_id);
if((scenario==1||scenario==6)&&record->accepted&&!canceled) {
if((scenario==1||scenario==6||scenario==9)&&record->accepted&&!canceled) {
registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true;
}
if(record->result) break;
@@ -51,7 +51,7 @@ int main(int argc,char** argv) {
if(!record||!record->result) throw std::runtime_error("result timeout");
const auto& result=*record->result;
bool blocked_redispatch=false;
if(scenario==6) {
if(scenario==6||scenario==9||scenario==10) {
auto next=request;next.goal_id+="-forbidden-retry";
next.trace.subtask_id="navigate-forbidden-retry";++next.trace.attempt;
blocked_redispatch=!registry.start(next,robot_bt::SteadyClock::now()).has_value();