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();
ros2/bt_mock_servers/bt_mock_servers/mock_skills.py
+139 -69
View File
@@ -19,20 +19,21 @@ from rclpy.node import Node
from bt_skill_interfaces.action import (
AssessGrasp, CheckFreeSpace, ExecuteManipulation, ExecutePosture,
EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn,
Navigate, NavigateSemantic, PlanTask, VerifyState,
PlanTask, VerifyState,
)
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, NavigationResult, RobotState, SafetyState,
from navigation_interfaces.action import NavigateToPose
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState,
VerificationEvidence, VisualObservation)
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
from std_msgs.msg import String
from .scenarios import duration_seconds, fixture_at, fixed_plan, parse_scenarios, strict_json, validate_trace
ACTION_TYPES = {
"navigate": Navigate, "execute_manipulation": ExecuteManipulation,
"navigate": NavigateToPose, "execute_manipulation": ExecuteManipulation,
"plan_task": PlanTask, "locate_shelf_column": LocateShelfColumn,
"localize_target_3d": LocalizeTarget3D, "check_free_space": CheckFreeSpace,
"assess_grasp": AssessGrasp, "execute_posture": ExecutePosture, "verify_state": VerifyState,
"navigate_semantic": NavigateSemantic, "evaluate_progress": EvaluateProgress,
"evaluate_progress": EvaluateProgress,
"execute_task": ExecuteTask,
}
ACTION_ENDPOINTS = {
@@ -41,10 +42,10 @@ ACTION_ENDPOINTS = {
"execute_task": "tasks/execute",
"evaluate_progress": "monitor/evaluate_progress",
}
MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task"))
MOTION = frozenset(("navigate", "execute_manipulation", "execute_posture", "execute_task"))
SUCCESS_PHASES = {
"navigate": (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING),
"navigate": (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING,
NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING),
"execute_manipulation": (ExecuteManipulation.Feedback.PREPARING,
ExecuteManipulation.Feedback.WAITING_OBSERVATION,
ExecuteManipulation.Feedback.INFERRING,
@@ -55,7 +56,7 @@ SUCCESS_PHASES = {
"assess_grasp": (0,), "execute_posture": (ExecutePosture.Feedback.CHECKING,
ExecutePosture.Feedback.MOVING,
ExecutePosture.Feedback.SETTLING),
"verify_state": (0,), "navigate_semantic": (0,), "evaluate_progress": (0,),
"verify_state": (0,), "evaluate_progress": (0,),
}
@@ -80,13 +81,13 @@ def normalized_navigation_pose(target_pose):
def lifecycle_phases(name, kind):
if name == "navigate" and kind == "obstacle_recovery":
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING, Navigate.Feedback.BLOCKED,
Navigate.Feedback.NAVIGATING)
return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING,
NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING, NavigateToPose.Feedback.BLOCKED,
NavigateToPose.Feedback.NAVIGATING)
if name == "navigate" and kind == "blocked":
return (*SUCCESS_PHASES[name], Navigate.Feedback.BLOCKED)
return (*SUCCESS_PHASES[name], NavigateToPose.Feedback.BLOCKED)
if name == "navigate" and kind == "not_ready":
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING)
return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING)
return SUCCESS_PHASES.get(name, (0,))
@@ -199,15 +200,6 @@ class MockSkills(Node):
destination = (bool(request.destination.region_ref), bool(request.destination.description))
if destination != ((False, False) if request.skill == "pick" else (True, True)):
raise ValueError("destination must be empty for pick and complete for place")
elif name == "navigate_semantic":
if request.kind not in ("LOCATION", "OBJECT", "CELL") or not request.reference or not request.registry_version:
raise ValueError("semantic navigation binding is invalid")
if request.kind == "CELL" and any(
not isinstance(value, str) or not value or "/" in value
for value in (request.shelf_id, request.side_id, request.column_id, request.tier_id)):
raise ValueError("semantic cell binding is incomplete")
if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
raise ValueError("semantic navigation tolerances are invalid")
elif name == "execute_posture":
if request.posture_id not in self.allowed_postures or not request.expected_geometry_epoch:
raise ValueError("posture or geometry epoch is invalid")
@@ -281,10 +273,18 @@ class MockSkills(Node):
self.counts[name] += 1
self.accepted[goal_id] = (time.monotonic(), fixture)
if name in MOTION:
self.motion_owner = (goal_id, name, copy.deepcopy(handle.request.trace))
self.motion_owner = (goal_id, name, self._goal_identity(name, handle.request))
handle.execute()
@staticmethod
def _goal_identity(name, request):
if name == "navigate":
return (request.task_id, request.subtask_id)
return copy.deepcopy(request.trace)
def _execute(self, name, handle):
if name == "navigate":
return self._execute_navigation(handle)
goal_id = bytes(handle.goal_id.uuid).hex()
with self.lock:
started, fixture = self.accepted.pop(goal_id)
@@ -324,7 +324,7 @@ class MockSkills(Node):
self._feedback(name, handle, sequence, now - started, phases[phase_index])
emitted_phase = phase_index
time.sleep(min(0.02, max(0, deadline - now)))
if (kind == "failed" or name == "navigate" and kind in ("blocked", "not_ready")) and outcome == ExecutionResult.COMPLETED:
if kind == "failed" and outcome == ExecutionResult.COMPLETED:
outcome = ExecutionResult.FAILED
if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED:
outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN
@@ -334,7 +334,7 @@ class MockSkills(Node):
# Unknown execution state stays reserved. A client cannot infer stop from this exception.
stop_state = ExecutionResult.UNKNOWN if name in MOTION else ExecutionResult.CONFIRMED
if hasattr(result, "result"):
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc), name=name)
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc))
elif hasattr(result, "evidence"):
result.evidence.status = VerificationEvidence.UNKNOWN
result.evidence.error_code = "MOCK_EXCEPTION"
@@ -357,7 +357,109 @@ class MockSkills(Node):
if self.motion_owner and self.motion_owner[0] == goal_id:
self.motion_owner = None
elif name in MOTION:
self.unresolved_motion[goal_id] = (name, copy.deepcopy(handle.request.trace))
self.unresolved_motion[goal_id] = (name, self._goal_identity(name, handle.request))
return result
def _execute_navigation(self, handle):
"""Explicit simulator backend producing only native navigation messages."""
goal_id = bytes(handle.goal_id.uuid).hex()
with self.lock:
started, fixture = self.accepted.pop(goal_id)
kind = fixture.get("kind", "normal")
deadline = started + duration_seconds(handle.request.timeout)
finish = started + fixture.get("duration_seconds", 0.2)
result = NavigateToPose.Result()
result.status = result.SUCCEEDED
result.stop_state = result.STOP_UNKNOWN
sequence, emitted_phase = 0, -1
try:
while True:
now = time.monotonic()
if handle.is_cancel_requested:
result.status = result.CANCELED
self._stopping_feedback("navigate", handle, sequence + 1, now - started)
time.sleep(fixture.get("stop_delay_seconds", 0.0))
break
if now >= deadline or not rclpy.ok():
result.status = result.TIMEOUT
self._stopping_feedback("navigate", handle, sequence + 1, now - started)
break
if now >= finish and kind not in ("timeout", "timeout_stop_unknown", "silence"):
break
if kind != "silence":
phases = lifecycle_phases("navigate", kind)
phase_index = min(len(phases) - 1, int((now - started) / max(0.001, finish - started) * len(phases)))
if phase_index > emitted_phase:
sequence += 1
self._feedback("navigate", handle, sequence, now - started, phases[phase_index])
emitted_phase = phase_index
time.sleep(min(0.02, max(0, deadline - now)))
if result.status == result.SUCCEEDED:
if kind in ("failed", "stop_unknown"):
result.status = result.FAILED
elif kind == "blocked":
result.status = result.BLOCKED
elif kind == "not_ready":
result.status = result.NOT_READY
if kind not in ("stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"):
# This dedicated simulator explicitly observes its own stopped state.
result.stop_state = result.STOP_CONFIRMED
result.stopped_at = self.get_clock().now().to_msg()
result.stop_evidence_ref = "sim://stop/" + goal_id
result.error_code = "" if result.status == result.SUCCEEDED else "MOCK_TERMINATED"
if result.status == result.BLOCKED:
result.error_code = "BLOCKED"
elif result.status == result.NOT_READY:
result.error_code = fixture.get("error_code", "INPUTS_UNHEALTHY")
result.message = "SIMULATED navigation only"
result.final_pose_valid = result.status == result.SUCCEEDED
if result.final_pose_valid:
result.final_pose = normalized_navigation_pose(handle.request.target_pose)
result.final_pose.header.stamp = self.get_clock().now().to_msg()
pose_fixture = fixture.get("final_pose")
if pose_fixture is not None:
result.final_pose.header.frame_id = pose_fixture["frame_id"]
for field in ("x", "y", "z"):
setattr(result.final_pose.pose.position, field, float(pose_fixture[field]))
for field in ("x", "y", "z", "w"):
setattr(result.final_pose.pose.orientation, field, float(pose_fixture["q" + field]))
actual, target = result.final_pose.pose, normalized_navigation_pose(handle.request.target_pose).pose
result.final_position_error = math.hypot(actual.position.x - target.position.x,
actual.position.y - target.position.y,
actual.position.z - target.position.z)
def yaw(q):
return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
delta = yaw(target.orientation) - yaw(actual.orientation)
result.final_yaw_error = math.atan2(math.sin(delta), math.cos(delta))
if result.stop_state == result.STOP_CONFIRMED:
with self.lock:
self.geometry_epoch += 1
if kind == "native_mismatch":
handle.abort()
elif result.status == result.CANCELED:
handle.canceled()
elif result.status == result.SUCCEEDED:
handle.succeed()
else:
handle.abort()
except Exception as exc:
result.status, result.stop_state = result.FAILED, result.STOP_UNKNOWN
result.error_code, result.message = "MOCK_EXCEPTION", str(exc)
result.stopped_at, result.stop_evidence_ref = Time(), ""
result.final_pose_valid = False
if handle.is_active:
handle.abort()
self.get_logger().error("Mock navigation exception: " + str(exc))
finally:
with self.lock:
self.inflight -= 1
if result.stop_state == result.STOP_CONFIRMED:
self.motion_reserved = False
self.unresolved_motion.pop(goal_id, None)
if self.motion_owner and self.motion_owner[0] == goal_id:
self.motion_owner = None
else:
self.unresolved_motion[goal_id] = ("navigate", self._goal_identity("navigate", handle.request))
return result
@staticmethod
@@ -390,30 +492,22 @@ class MockSkills(Node):
feedback.error_valid = True
feedback.position_error = 0.0
feedback.yaw_error = 0.0
feedback.blocked = feedback.phase == Navigate.Feedback.BLOCKED
feedback.blocked_valid = True
feedback.blocked = feedback.phase == NavigateToPose.Feedback.BLOCKED
if name == "execute_manipulation":
feedback.progress_valid = False
handle.publish_feedback(feedback)
def _stopping_feedback(self, name, handle, sequence, elapsed):
stopping = {"navigate": Navigate.Feedback.STOPPING,
stopping = {"navigate": NavigateToPose.Feedback.STOPPING,
"execute_manipulation": ExecuteManipulation.Feedback.STOPPING,
"execute_posture": ExecutePosture.Feedback.STOPPING}.get(name)
if stopping is not None:
self._feedback(name, handle, sequence, elapsed, stopping)
def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only", name="", kind="normal"):
result = NavigationResult() if name == "navigate" else ExecutionResult()
if name == "navigate":
status = {ExecutionResult.COMPLETED: NavigationResult.SUCCEEDED,
ExecutionResult.CANCELED: NavigationResult.CANCELED,
ExecutionResult.TIMED_OUT: NavigationResult.TIMEOUT,
ExecutionResult.FAILED: NavigationResult.FAILED}[outcome]
if outcome == ExecutionResult.FAILED and kind in ("blocked", "not_ready"):
status = NavigationResult.BLOCKED if kind == "blocked" else NavigationResult.NOT_READY
else:
status = outcome
result.status, result.stop_state = status, stop_state
def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"):
result = ExecutionResult()
result.status, result.stop_state = outcome, stop_state
result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED")
result.message = message
if stop_state == ExecutionResult.CONFIRMED:
@@ -430,34 +524,8 @@ class MockSkills(Node):
record = "sim://" + name + "/" + goal_id
ok = outcome == ExecutionResult.COMPLETED
if hasattr(result, "result"):
error = ""
if name == "navigate" and outcome == ExecutionResult.FAILED:
if kind == "blocked": error = "BLOCKED"
if kind == "not_ready": error = fixture.get("error_code", "INPUTS_UNHEALTHY")
result.result = self._execution_result(outcome, stop_state, goal_id, error, name=name, kind=kind)
if name in ("navigate", "navigate_semantic"):
if ok and stop_state == ExecutionResult.CONFIRMED:
with self.lock:
self.geometry_epoch += 1
if name == "navigate":
result.final_pose_valid = ok
if ok:
result.final_pose = normalized_navigation_pose(request.target_pose)
result.final_position_error = result.final_yaw_error = 0.0
else:
pose = fixture.get("final_pose")
result.pose_valid = result.errors_valid = ok and pose is not None
if pose is not None:
result.final_pose.header.frame_id = pose["frame_id"]
result.final_pose.pose.position.x = pose["x"]
result.final_pose.pose.position.y = pose["y"]
result.final_pose.pose.position.z = pose["z"]
result.final_pose.pose.orientation.x = pose["qx"]
result.final_pose.pose.orientation.y = pose["qy"]
result.final_pose.pose.orientation.z = pose["qz"]
result.final_pose.pose.orientation.w = pose["qw"]
result.final_pose.header.stamp = observed
elif name == "execute_manipulation":
result.result = self._execution_result(outcome, stop_state, goal_id)
if name == "execute_manipulation":
result.execution_record_ref = record
# Deliberately no holding/verification state mutation here.
elif name == "plan_task":
@@ -725,10 +793,12 @@ class MockSkills(Node):
fresh = 0 < observed_ns <= now < valid_until_ns
with self.lock:
owned = self.unresolved_motion.get(request.goal_id)
owned_trace_matches = owned is not None and all(
owned_trace_matches = owned is not None and owned[0] != "navigate" and all(
getattr(owned[1], field) == getattr(request.trace, field)
for field in ("task_id", "subtask_id", "attempt", "task_revision",
"plan_version", "run_id", "execution_generation"))
if owned is not None and owned[0] == "navigate":
owned_trace_matches = owned[1] == (request.trace.task_id, request.trace.subtask_id)
owner_matches = self.motion_owner is not None and self.motion_owner[0] == request.goal_id
bound = (owner_matches and owned_trace_matches and evidence.context.source_goal_id == request.goal_id and
same_trace and evidence.status == evidence.PASSED and
ros2/bt_mock_servers/bt_mock_servers/scenarios.py
+5 -6
View File
@@ -6,7 +6,7 @@ ACTION_NAMES = (
"navigate", "execute_manipulation", "plan_task", "locate_shelf_column",
"localize_target_3d", "check_free_space", "assess_grasp", "execute_posture",
"verify_state",
"navigate_semantic", "evaluate_progress", "execute_task",
"evaluate_progress", "execute_task",
"robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry",
)
KINDS = {
@@ -32,7 +32,6 @@ FIXTURE_FIELDS = {
BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"}
ACTION_KINDS = {
"navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery", "blocked", "not_ready"},
"navigate_semantic": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_manipulation": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_posture": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_task": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
@@ -89,7 +88,7 @@ def parse_scenarios(raw):
if "error_code" in fixture:
code = fixture["error_code"]
if (name != "navigate" or fixture.get("kind") != "not_ready" or
code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "EXECUTION_BACKEND_NOT_CONFIGURED",
"ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED"}):
raise ValueError("error_code requires a supported navigation readiness reason")
delay = fixture.get("duration_seconds", 0.2)
@@ -115,12 +114,12 @@ def parse_scenarios(raw):
if "final_pose" in fixture:
pose = fixture["final_pose"]
required = {"frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"}
if not isinstance(pose, dict) or set(pose) != required or not isinstance(pose["frame_id"], str) or not pose["frame_id"]:
raise ValueError("final_pose requires an exact frame and pose")
if name != "navigate" or not isinstance(pose, dict) or set(pose) != required or pose["frame_id"] != "map":
raise ValueError("final_pose requires a navigation pose in map")
values = [pose[key] for key in ("x", "y", "z", "qx", "qy", "qz", "qw")]
if any(type(value) not in (int, float) or not math.isfinite(value) for value in values):
raise ValueError("final_pose values must be finite")
if abs(sum(pose[key] ** 2 for key in ("qx", "qy", "qz", "qw")) - 1.0) > 0.001:
if abs(math.hypot(*(pose[key] for key in ("qx", "qy", "qz", "qw"))) - 1.0) > 0.001:
raise ValueError("final_pose quaternion must have unit norm")
result[name] = entries
return result
ros2/bt_mock_servers/package.xml
+1
View File
@@ -8,6 +8,7 @@
<buildtool_depend>ament_python</buildtool_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>bt_skill_interfaces</exec_depend>
<exec_depend>navigation_interfaces</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>std_msgs</exec_depend>
<export><build_type>ament_python</build_type></export>
ros2/bt_skill_interfaces/CMakeLists.txt
-3
View File
@@ -10,7 +10,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ObjectTarget.msg"
"msg/RegionTarget.msg"
"msg/ExecutionResult.msg"
"msg/NavigationResult.msg"
"msg/ObservationContext.msg"
"msg/RobotState.msg"
"msg/SafetyState.msg"
@@ -18,8 +17,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Station.msg"
"msg/TargetBinding.msg"
"msg/PlacementBinding.msg"
"action/Navigate.action"
"action/NavigateSemantic.action"
"action/EvaluateProgress.action"
"msg/VisualObservation.msg"
"msg/DenseProgress.msg"
ros2/bt_skill_interfaces/action/NavigateSemantic.action
-24
View File
@@ -1,24 +0,0 @@
# v1.1 navigation-owned lookup; no model-supplied pose.
bt_skill_interfaces/TaskTrace trace
string kind
string reference
string shelf_id
string side_id
string column_id
string tier_id
uint32 registry_version
float32 position_tolerance
float32 orientation_tolerance
builtin_interfaces/Duration timeout
---
bt_skill_interfaces/ExecutionResult result
geometry_msgs/PoseStamped final_pose
bool pose_valid
float32 final_position_error
float32 final_orientation_error
bool errors_valid
---
builtin_interfaces/Time stamp
uint32 sequence
uint8 phase
string message
ros2/bt_skill_interfaces/msg/NavigationResult.msg
-15
View File
@@ -1,15 +0,0 @@
# Navigation-specific outcomes; do not decode with ExecutionResult enum values.
uint8 SUCCEEDED=0
uint8 CANCELED=1
uint8 TIMEOUT=2
uint8 BLOCKED=3
uint8 NOT_READY=4
uint8 FAILED=5
uint8 UNKNOWN=0
uint8 CONFIRMED=1
uint8 status
string error_code
string message
uint8 stop_state
builtin_interfaces/Time stopped_at
string stop_evidence_ref
ros2/bt_skill_interfaces/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<package format="3">
<name>bt_skill_interfaces</name>
<version>2.0.0</version>
<description>Robot skill contracts with navigation-specific outcomes and stop evidence.</description>
<description>Robot task and manipulation contracts with stop evidence.</description>
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer>
<license>Proprietary</license>
<buildtool_depend>ament_cmake</buildtool_depend>
ros2/navigation_interfaces/CMakeLists.txt
+12
View File
@@ -0,0 +1,12 @@
cmake_minimum_required(VERSION 3.8)
project(navigation_interfaces)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/NavigateToPose.action"
DEPENDENCIES builtin_interfaces geometry_msgs
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
ros2/bt_skill_interfaces/action/Navigate.action → ros2/navigation_interfaces/action/NavigateToPose.action
+22 -8
View File
@@ -1,30 +1,44 @@
# Navigation contract: trace and stop evidence retained across all attempts.
# Enum values are explicit; both peers must build this same interface package.
bt_skill_interfaces/TaskTrace trace
string task_id
string subtask_id
geometry_msgs/PoseStamped target_pose
float64 position_tolerance
float64 yaw_tolerance
builtin_interfaces/Duration timeout
---
bt_skill_interfaces/NavigationResult result
uint8 SUCCEEDED=0
uint8 CANCELED=1
uint8 TIMEOUT=2
uint8 BLOCKED=3
uint8 NOT_READY=4
uint8 FAILED=5
uint8 STOP_UNKNOWN=0
uint8 STOP_CONFIRMED=1
uint8 status
string error_code
string message
bool final_pose_valid
geometry_msgs/PoseStamped final_pose
float64 final_position_error
float64 final_yaw_error
uint8 stop_state
builtin_interfaces/Time stopped_at
string stop_evidence_ref
---
uint8 ACCEPTED=0
uint8 CHECKING=1
uint8 NAVIGATING=2
uint8 BLOCKED=3
uint8 STOPPING=4
uint8 PLANNING=2
uint8 NAVIGATING=3
uint8 BLOCKED=4
uint8 STOPPING=5
builtin_interfaces/Time stamp
uint32 sequence
uint64 sequence
uint8 phase
bool current_pose_valid
geometry_msgs/PoseStamped current_pose
bool error_valid
float64 position_error
float64 yaw_error
bool blocked_valid
bool blocked
builtin_interfaces/Duration elapsed_time
string message
ros2/navigation_interfaces/package.xml
+12
View File
@@ -0,0 +1,12 @@
<?xml version="1.0"?>
<package format="3">
<name>navigation_interfaces</name><version>1.0.0</version>
<description>Navigation action contract.</description>
<maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>builtin_interfaces</depend><depend>geometry_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export><build_type>ament_cmake</build_type></export>
</package>