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/src/ros_driver.cpp
+25 -7
View File
@@ -170,6 +170,24 @@ 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;
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;}
}
return execution(common,request);
}
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;
// These servers are contractually read-only. Their native terminal is enough
@@ -210,13 +228,13 @@ void RosDriver::send(const GoalRequest& r) {
});break;
}
Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
send_typed<Navigate>(navigate_,g,r,6,[this,r](const Navigate::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&&std::abs(m.final_orientation_error)<=std::acos(-1.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);
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&&
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;
if(m.final_pose_valid)out.response.final_pose=pose_core(m.final_pose);
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
}
case Skill::PICK:case Skill::PLACE: {