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/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;