feat: add release-to-move TCP gizmo control in Webots
Add a native Webots marker for setting the active tool's TCP position and orientation. Dragging edits the preview; releasing the mouse submits one complete, checked joint trajectory with configured TCP speed limits. Unreachable targets reset the marker to the current TCP. Preserve coordinate, joint, and control-file commands through motion reservations and a controller action proxy. Give the gizmo lowest priority and finish its committed trajectory before dispatching queued regular commands, including waits longer than the former two-second service timeout. Place the patron TCP at its working end, 55.2 mm along local Z, and use the flange TCP when no tool is attached. Add digital_twin.tcp_gizmo configuration, ROS interfaces, launch integration, and regression coverage. Document operation, configuration, tool attachment, speed limits, and troubleshooting in both Russian and English. Validation: 42 Python tests and 8 C++ tests passed. Webots regressions covered one action per release, translation and rotation with and without a tool, unreachable targets, and ordinary commands waiting for gizmo completion.
This commit is contained in:
Даниил Грабарь
@@ -9,6 +9,8 @@ endif()
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(moveit_ros_planning_interface REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
@@ -16,6 +18,16 @@ find_package(moveit_ros_planning REQUIRED)
|
||||
find_package(moveit_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(iiwa_msgs REQUIRED)
|
||||
find_package(moveit_servo REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(action_msgs REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
|
||||
add_executable(tcp_gizmo src/tcp_gizmo.cpp)
|
||||
target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
|
||||
target_include_directories(tcp_gizmo PRIVATE include)
|
||||
ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
|
||||
action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
|
||||
|
||||
add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
|
||||
target_include_directories(motion_planning_cpp PUBLIC include)
|
||||
@@ -33,6 +45,7 @@ ament_target_dependencies(motion_planning_cpp
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
||||
install(TARGETS
|
||||
tcp_gizmo
|
||||
motion_planning_cpp
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
@@ -55,4 +68,14 @@ install(FILES
|
||||
)
|
||||
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(ament_cmake_pytest REQUIRED)
|
||||
ament_add_gtest(test_tcp_gizmo_policy test/test_tcp_gizmo_policy.cpp)
|
||||
target_compile_features(test_tcp_gizmo_policy PUBLIC cxx_std_17)
|
||||
target_include_directories(test_tcp_gizmo_policy PRIVATE include)
|
||||
ament_add_pytest_test(test_motion_priority test/test_motion_priority.py
|
||||
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR})
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
"""Wall-clock motion reservations and serialization, independent of ROS simulation time."""
|
||||
import threading
|
||||
import uuid
|
||||
|
||||
|
||||
class LeaseError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
class MotionLease:
|
||||
"""Renew one owner until context exit; transport errors fail closed."""
|
||||
def __init__(self, exchange=None, *, on_lost=lambda: None, ttl=5.0,
|
||||
renew_interval=1.0):
|
||||
self.owner = uuid.uuid4().hex
|
||||
self._exchange = exchange
|
||||
self._on_lost = on_lost
|
||||
self._ttl = ttl
|
||||
self._interval = renew_interval
|
||||
self._stop = threading.Event()
|
||||
self._lost = threading.Event()
|
||||
self._thread = None
|
||||
|
||||
def __enter__(self):
|
||||
if self._exchange is not None:
|
||||
self._exchange(self.owner, True, self._ttl)
|
||||
self._thread = threading.Thread(target=self._renew, daemon=True)
|
||||
self._thread.start()
|
||||
return self
|
||||
|
||||
def _renew(self):
|
||||
while not self._stop.wait(self._interval):
|
||||
try:
|
||||
self._exchange(self.owner, True, self._ttl)
|
||||
except Exception:
|
||||
self._lost.set()
|
||||
try:
|
||||
self._on_lost()
|
||||
finally:
|
||||
return
|
||||
|
||||
@property
|
||||
def lost(self):
|
||||
return self._lost.is_set()
|
||||
|
||||
def check(self):
|
||||
if self.lost:
|
||||
raise LeaseError('Motion priority reservation lost')
|
||||
|
||||
def __exit__(self, exc_type, exc, tb):
|
||||
self._stop.set()
|
||||
if self._thread is not None:
|
||||
self._thread.join(timeout=3.0)
|
||||
try:
|
||||
self._exchange(self.owner, False, self._ttl)
|
||||
except Exception:
|
||||
# Expiry bounds cleanup even if the service disappeared.
|
||||
pass
|
||||
if exc_type is None:
|
||||
self.check()
|
||||
return False
|
||||
|
||||
|
||||
class MotionSerialiser:
|
||||
"""One PlanningComponent owner; stop invalidates both planning and queued work."""
|
||||
def __init__(self):
|
||||
self.lock = threading.Lock()
|
||||
self._state_lock = threading.Lock()
|
||||
self._generation = 0
|
||||
|
||||
def generation(self):
|
||||
with self._state_lock:
|
||||
return self._generation
|
||||
|
||||
def check(self, generation):
|
||||
if generation != self.generation():
|
||||
raise LeaseError('Motion interrupted by stop')
|
||||
|
||||
def stop(self, callback):
|
||||
with self._state_lock:
|
||||
self._generation += 1
|
||||
callback()
|
||||
|
||||
|
||||
def wait_response(future, timeout=2.0, *, available=lambda: True):
|
||||
"""Wait only from worker threads; an independent executor services the future."""
|
||||
done = threading.Event()
|
||||
future.add_done_callback(lambda _: done.set())
|
||||
if timeout is None:
|
||||
# Acquisition is acknowledged only after a committed gizmo trajectory
|
||||
# finishes. Its duration can exceed the normal service RPC timeout.
|
||||
while not done.wait(.2):
|
||||
if not available():
|
||||
future.cancel()
|
||||
raise LeaseError('Motion priority service unavailable while waiting')
|
||||
elif not done.wait(timeout):
|
||||
future.cancel()
|
||||
raise LeaseError('Motion priority service response timed out')
|
||||
result = future.result()
|
||||
if result is None:
|
||||
raise LeaseError('Motion priority service returned no response')
|
||||
return result
|
||||
|
||||
|
||||
class MotionPriorityClient:
|
||||
"""Dedicated executor prevents lease starvation under concurrent motion requests."""
|
||||
def __init__(self, *, context=None):
|
||||
from rclpy.node import Node
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from iiwa_msgs.srv import MotionPriority
|
||||
self._type = MotionPriority
|
||||
self._node = Node('motion_priority_' + uuid.uuid4().hex[:12], context=context,
|
||||
start_parameter_services=False, use_global_arguments=False)
|
||||
self._client = self._node.create_client(
|
||||
MotionPriority, '/cobot/tcp_gizmo/priority',
|
||||
callback_group=ReentrantCallbackGroup())
|
||||
self._executor = MultiThreadedExecutor(num_threads=2, context=context)
|
||||
self._executor.add_node(self._node)
|
||||
self._thread = threading.Thread(target=self._spin, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _spin(self):
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
try:
|
||||
self._executor.spin()
|
||||
except ExternalShutdownException:
|
||||
pass
|
||||
|
||||
def create_stop_service(self, callback):
|
||||
from std_srvs.srv import Trigger
|
||||
return self._node.create_service(Trigger, "/cobot/stop", callback)
|
||||
|
||||
def exchange(self, owner, acquire, ttl):
|
||||
if not self._client.wait_for_service(timeout_sec=0.5):
|
||||
raise LeaseError('Motion priority service unavailable')
|
||||
request = self._type.Request(owner=owner, acquire=acquire, ttl=ttl)
|
||||
result = wait_response(
|
||||
self._client.call_async(request), timeout=None if acquire else 2.0,
|
||||
available=lambda: self._node.context.ok() and self._client.service_is_ready())
|
||||
if not result.success:
|
||||
raise LeaseError(result.message)
|
||||
|
||||
def close(self):
|
||||
self._executor.shutdown(timeout_sec=2.0)
|
||||
self._thread.join(timeout=2.0)
|
||||
self._node.destroy_node()
|
||||
@@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
namespace iiwa_planning {
|
||||
inline bool requiresGizmoMotion(double translation_error, double angular_error) {
|
||||
return translation_error >= 1e-5 || angular_error >= 1e-4;
|
||||
}
|
||||
|
||||
// Pure arbitration state. The ROS node serializes all access. Steady time is
|
||||
// used for leases/commands; simulation time is used for trajectory deadlines.
|
||||
class GizmoPolicy {
|
||||
public:
|
||||
std::uint64_t epoch() const { return epoch_; }
|
||||
bool hasTarget() const { return target_; }
|
||||
void invalidate() { target_ = false; ++epoch_; }
|
||||
bool accept(std::uint64_t epoch, double now, double timeout) {
|
||||
if (epoch != epoch_) return false;
|
||||
target_ = true; target_deadline_ = now + timeout; return true;
|
||||
}
|
||||
bool expired(double now) const { return target_ && now >= target_deadline_; }
|
||||
bool acquire(const std::string& owner, double ttl, double now) {
|
||||
if (owner.empty() || !std::isfinite(ttl) || ttl <= 0. || ttl > 60.) return false;
|
||||
const auto it = leases_.find(owner);
|
||||
if (it == leases_.end() || it->second <= now) invalidate();
|
||||
leases_[owner] = now + ttl; settling_ = true; return true;
|
||||
}
|
||||
void release(const std::string& owner) { leases_.erase(owner); }
|
||||
void normal(double deadline) { invalidate(); normal_deadline_ = deadline; settling_ = true; }
|
||||
void action(bool active) {
|
||||
if (active && !action_active_) { invalidate(); settling_ = true; }
|
||||
action_active_ = active;
|
||||
}
|
||||
bool allowed(double steady, double sim, bool settled) {
|
||||
for (auto it = leases_.begin(); it != leases_.end();) {
|
||||
if (it->second <= steady) it = leases_.erase(it); else ++it;
|
||||
}
|
||||
if (!leases_.empty() || action_active_ || sim < normal_deadline_) return false;
|
||||
if (settling_ && !settled) return false;
|
||||
settling_ = false; return true;
|
||||
}
|
||||
private:
|
||||
std::uint64_t epoch_{0};
|
||||
bool target_{false}, action_active_{false}, settling_{false};
|
||||
double target_deadline_{0.}, normal_deadline_{0.};
|
||||
std::map<std::string, double> leases_;
|
||||
};
|
||||
// A controller action owns the low slot from submission through its terminal
|
||||
// result. Releasing the mouse or requesting high-priority motion must not
|
||||
// release this slot before the already dispatched bounded step finishes.
|
||||
class LowGoalBarrier {
|
||||
public:
|
||||
bool idle() const { return !pending_; }
|
||||
bool begin() {
|
||||
if (pending_) return false;
|
||||
pending_ = true; return true;
|
||||
}
|
||||
void accepted(bool value) { if (!value) terminal(); }
|
||||
void terminal() { pending_ = false; }
|
||||
private:
|
||||
bool pending_{false};
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
@@ -11,6 +11,8 @@
|
||||
<buildtool_depend>ament_cmake_python</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
<depend>moveit_core</depend>
|
||||
@@ -25,9 +27,19 @@
|
||||
<depend>tf_transformations</depend>
|
||||
<depend>python3-scipy</depend>
|
||||
<depend>iiwa_msgs</depend>
|
||||
<depend>moveit_servo</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
<depend>action_msgs</depend>
|
||||
<depend>rcl_interfaces</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>iiwa_utils</test_depend>
|
||||
<test_depend>controller_manager_msgs</test_depend>
|
||||
<test_depend>python3-numpy</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
||||
@@ -28,6 +28,12 @@ import rosbag2_py
|
||||
from rosidl_runtime_py.utilities import get_message
|
||||
|
||||
from iiwa_msgs.action import MoveToJoints, MoveToPose
|
||||
from rcl_interfaces.srv import GetParameters
|
||||
from rcl_interfaces.msg import ParameterType
|
||||
from std_srvs.srv import Trigger
|
||||
from iiwa_planning.motion_priority import (
|
||||
MotionLease, MotionPriorityClient, LeaseError, wait_response,
|
||||
)
|
||||
|
||||
|
||||
def _is_joints_waypoint(wp: dict) -> bool:
|
||||
@@ -70,6 +76,15 @@ class MotionSequenceRunner(Node):
|
||||
)
|
||||
self.get_logger().info(f'joints_action={joints_action} pose_action={pose_action}')
|
||||
|
||||
self._parameters_client = self.create_client(
|
||||
GetParameters, '/iiwa_motion_server/get_parameters', callback_group=self._cb_group)
|
||||
self._stop_client = self.create_client(
|
||||
Trigger, '/cobot/stop', callback_group=self._cb_group)
|
||||
self._priority = None
|
||||
self._lease_active = None
|
||||
self._aborted = threading.Event()
|
||||
self._active_goal = None
|
||||
self.exit_code = 1
|
||||
self._writer: rosbag2_py.SequentialWriter | None = None
|
||||
self._registered_topics: set[str] = set()
|
||||
self._subs = []
|
||||
@@ -180,6 +195,9 @@ class MotionSequenceRunner(Node):
|
||||
|
||||
def _on_goal(future):
|
||||
gh = future.result()
|
||||
self._active_goal = gh
|
||||
if self._aborted.is_set() and gh.accepted:
|
||||
gh.cancel_goal_async()
|
||||
if not gh.accepted:
|
||||
self.get_logger().error('MoveToJoints goal rejected')
|
||||
done.set()
|
||||
@@ -187,8 +205,15 @@ class MotionSequenceRunner(Node):
|
||||
gh.get_result_async().add_done_callback(_on_result)
|
||||
|
||||
self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
|
||||
done.wait()
|
||||
return result_holder[0]
|
||||
deadline = time.monotonic() + 300.0
|
||||
while not done.wait(0.1):
|
||||
if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
|
||||
self._abort_sequence()
|
||||
return False
|
||||
if self._lease_active:
|
||||
self._lease_active.check()
|
||||
self._active_goal = None
|
||||
return result_holder[0] and not self._aborted.is_set()
|
||||
|
||||
def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
|
||||
goal = MoveToPose.Goal()
|
||||
@@ -215,6 +240,9 @@ class MotionSequenceRunner(Node):
|
||||
|
||||
def _on_goal(future):
|
||||
gh = future.result()
|
||||
self._active_goal = gh
|
||||
if self._aborted.is_set() and gh.accepted:
|
||||
gh.cancel_goal_async()
|
||||
if not gh.accepted:
|
||||
self.get_logger().error('MoveToPose goal rejected')
|
||||
done.set()
|
||||
@@ -222,8 +250,15 @@ class MotionSequenceRunner(Node):
|
||||
gh.get_result_async().add_done_callback(_on_result)
|
||||
|
||||
self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
|
||||
done.wait()
|
||||
return result_holder[0]
|
||||
deadline = time.monotonic() + 300.0
|
||||
while not done.wait(0.1):
|
||||
if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
|
||||
self._abort_sequence()
|
||||
return False
|
||||
if self._lease_active:
|
||||
self._lease_active.check()
|
||||
self._active_goal = None
|
||||
return result_holder[0] and not self._aborted.is_set()
|
||||
|
||||
def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
|
||||
if _is_joints_waypoint(wp):
|
||||
@@ -231,21 +266,49 @@ class MotionSequenceRunner(Node):
|
||||
return self._send_pose_goal(wp, idx=idx)
|
||||
|
||||
|
||||
def _abort_sequence(self):
|
||||
self._aborted.set()
|
||||
if self._active_goal is not None and self._active_goal.accepted:
|
||||
self._active_goal.cancel_goal_async()
|
||||
# Asynchronous so renewal never waits on the motion server's callback pool.
|
||||
if self._stop_client.service_is_ready():
|
||||
self._stop_client.call_async(Trigger.Request())
|
||||
|
||||
def _discover_priority(self):
|
||||
if not self._parameters_client.wait_for_service(timeout_sec=5.0):
|
||||
raise LeaseError('Motion server parameter service unavailable')
|
||||
response = wait_response(self._parameters_client.call_async(
|
||||
GetParameters.Request(names=['tcp_gizmo_enabled'])))
|
||||
if len(response.values) != 1 or response.values[0].type != ParameterType.PARAMETER_BOOL:
|
||||
raise LeaseError('Motion server tcp_gizmo_enabled parameter unavailable')
|
||||
if response.values[0].bool_value:
|
||||
self._priority = MotionPriorityClient(context=self.context)
|
||||
|
||||
def run(self, done_event: threading.Event):
|
||||
self.exit_code = 1
|
||||
try:
|
||||
for i in range(self._n_iter):
|
||||
self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
|
||||
|
||||
self._send_waypoint(self._home)
|
||||
|
||||
for idx, wp in enumerate(self._waypoints):
|
||||
self._send_waypoint(wp, idx=idx)
|
||||
|
||||
self._send_waypoint(self._home)
|
||||
time.sleep(self._delay)
|
||||
|
||||
self._discover_priority()
|
||||
with MotionLease(self._priority.exchange if self._priority else None,
|
||||
on_lost=self._abort_sequence) as lease:
|
||||
self._lease_active = lease
|
||||
for i in range(self._n_iter):
|
||||
self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
|
||||
for idx, wp in enumerate([self._home, *self._waypoints, self._home]):
|
||||
lease.check()
|
||||
if self._aborted.is_set() or not self._send_waypoint(wp, idx=idx):
|
||||
raise LeaseError(f'Sequence interrupted at waypoint {idx}')
|
||||
if self._aborted.wait(max(0.0, self._delay)):
|
||||
raise LeaseError('Sequence reservation lost during pause')
|
||||
lease.check()
|
||||
self.get_logger().info('======= Sequence complete =======')
|
||||
self.exit_code = 0
|
||||
except Exception as exc:
|
||||
self._abort_sequence()
|
||||
self.get_logger().error(f'Sequence failed: {exc}')
|
||||
finally:
|
||||
self._lease_active = None
|
||||
if self._priority:
|
||||
self._priority.close()
|
||||
self.close_bag()
|
||||
done_event.set()
|
||||
|
||||
@@ -264,12 +327,16 @@ def main():
|
||||
while not done_event.is_set():
|
||||
executor.spin_once(timeout_sec=0.1)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
node._abort_sequence()
|
||||
deadline = time.monotonic() + 5.0
|
||||
while not done_event.is_set() and time.monotonic() < deadline:
|
||||
executor.spin_once(timeout_sec=0.1)
|
||||
finally:
|
||||
node.close_bag()
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
return node.exit_code
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
raise SystemExit(main())
|
||||
|
||||
@@ -17,6 +17,9 @@ from moveit.core.robot_state import RobotState
|
||||
|
||||
from iiwa_msgs.action import MoveToPose, MoveToJoints
|
||||
from iiwa_msgs.srv import MoveToNamedPose
|
||||
from iiwa_planning.motion_priority import (
|
||||
MotionLease, MotionPriorityClient, MotionSerialiser, LeaseError,
|
||||
)
|
||||
|
||||
|
||||
# Таблица планировщиков
|
||||
@@ -48,6 +51,10 @@ class IiwaMotionServer(Node):
|
||||
super().__init__("iiwa_motion_server")
|
||||
self._setup_parameters()
|
||||
self._setup_moveit()
|
||||
self._motion_serial = MotionSerialiser()
|
||||
self._operation = threading.local()
|
||||
self._priority = MotionPriorityClient(context=self.context)
|
||||
self._gizmo_enabled = self.get_parameter("tcp_gizmo_enabled").value
|
||||
self._setup_servers()
|
||||
|
||||
self.get_logger().info("Сервер движения iiwa запущен")
|
||||
@@ -59,6 +66,7 @@ class IiwaMotionServer(Node):
|
||||
self.declare_parameter("default_frame", "base_link")
|
||||
self.declare_parameter("default_planner", "ompl")
|
||||
self.declare_parameter("planning_attempts", 3)
|
||||
self.declare_parameter("tcp_gizmo_enabled", False)
|
||||
|
||||
self._pose_link = self.get_parameter("pose_link").value
|
||||
self._planning_group = self.get_parameter("planning_group").value
|
||||
@@ -67,7 +75,7 @@ class IiwaMotionServer(Node):
|
||||
self._planning_attempts = self.get_parameter("planning_attempts").value
|
||||
|
||||
def _setup_moveit(self):
|
||||
self._moveit = MoveItPy(node_name="iiwa_motion_server")
|
||||
self._moveit = MoveItPy(node_name="iiwa_motion_moveit")
|
||||
self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
|
||||
self._robot_model = self._moveit.get_robot_model()
|
||||
|
||||
@@ -75,19 +83,67 @@ class IiwaMotionServer(Node):
|
||||
cb = ReentrantCallbackGroup()
|
||||
|
||||
ActionServer(
|
||||
self, MoveToPose, "cobot/move_to_pose", self._execute_pose,
|
||||
self, MoveToPose, "cobot/move_to_pose", lambda gh: self._guard_action(gh, MoveToPose.Result, self._execute_pose),
|
||||
callback_group=cb,
|
||||
goal_callback=lambda _: GoalResponse.ACCEPT,
|
||||
cancel_callback=lambda _: CancelResponse.ACCEPT,
|
||||
)
|
||||
ActionServer(
|
||||
self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
|
||||
self, MoveToJoints, "cobot/move_to_joints", lambda gh: self._guard_action(gh, MoveToJoints.Result, self._execute_joints),
|
||||
callback_group=cb,
|
||||
goal_callback=lambda _: GoalResponse.ACCEPT,
|
||||
cancel_callback=lambda _: CancelResponse.ACCEPT,
|
||||
)
|
||||
self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
|
||||
self.create_service(Trigger, "cobot/stop", self._handle_stop, callback_group=cb)
|
||||
self.create_service(MoveToNamedPose, "cobot/move_to_named", self._guard_named, callback_group=cb)
|
||||
self._priority.create_stop_service(self._handle_stop)
|
||||
|
||||
def _stop_execution(self):
|
||||
self._motion_serial.stop(
|
||||
lambda: self._moveit.get_trajectory_execution_manager().stop_execution())
|
||||
|
||||
def _lease(self):
|
||||
return MotionLease(self._priority.exchange if self._gizmo_enabled else None,
|
||||
on_lost=self._stop_execution)
|
||||
|
||||
def _guarded(self, callback):
|
||||
generation = self._motion_serial.generation()
|
||||
# Reserve before waiting so queued requests also exclude the gizmo.
|
||||
with self._lease() as lease:
|
||||
with self._motion_serial.lock:
|
||||
self._motion_serial.check(generation)
|
||||
lease.check()
|
||||
self._operation.generation = generation
|
||||
self._operation.lease = lease
|
||||
return callback()
|
||||
|
||||
def _guard_action(self, goal_handle, result_type, callback):
|
||||
try:
|
||||
result, ok, message = self._guarded(lambda: callback(goal_handle))
|
||||
return self._finish_action(goal_handle, result, ok, message)
|
||||
except Exception as exc:
|
||||
result = result_type()
|
||||
result.success = False
|
||||
result.message = str(exc)
|
||||
if goal_handle.is_active:
|
||||
goal_handle.abort()
|
||||
return result
|
||||
|
||||
def _guard_named(self, request, response):
|
||||
try:
|
||||
return self._guarded(lambda: self._handle_named(request, response))
|
||||
except Exception as exc:
|
||||
response.success = False
|
||||
response.message = str(exc)
|
||||
return response
|
||||
|
||||
def _check_operation(self):
|
||||
self._operation.lease.check()
|
||||
self._motion_serial.check(self._operation.generation)
|
||||
|
||||
def destroy_node(self):
|
||||
if self._priority:
|
||||
self._priority.close()
|
||||
return super().destroy_node()
|
||||
|
||||
def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float, accel_scale: float | None = None) -> PlanRequestParameters:
|
||||
params = PlanRequestParameters(self._moveit, self._planning_group)
|
||||
@@ -105,19 +161,29 @@ class IiwaMotionServer(Node):
|
||||
Возвращает (True, msg) при успехе, (False, msg) при ошибке,
|
||||
(None, 'canceled') если цель была отменена.
|
||||
"""
|
||||
self._check_operation()
|
||||
plan_result = self._arm.plan(single_plan_parameters=plan_params)
|
||||
if not plan_result:
|
||||
return False, "Планирование не удалось: поза недостижима или в столкновении"
|
||||
|
||||
self._check_operation()
|
||||
if goal_handle.is_cancel_requested:
|
||||
return None, "canceled"
|
||||
|
||||
done = threading.Event()
|
||||
failed = threading.Event()
|
||||
|
||||
generation = self._operation.generation
|
||||
lease = self._operation.lease
|
||||
|
||||
def do_execute():
|
||||
try:
|
||||
self._moveit.execute(plan_result.trajectory, controllers=[])
|
||||
lease.check()
|
||||
self._motion_serial.check(generation)
|
||||
execution = self._moveit.execute(plan_result.trajectory, controllers=[])
|
||||
# Jazzy ExecutionStatus.__bool__ is true only for SUCCEEDED.
|
||||
if not bool(execution):
|
||||
failed.set()
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
|
||||
failed.set()
|
||||
@@ -126,14 +192,21 @@ class IiwaMotionServer(Node):
|
||||
|
||||
threading.Thread(target=do_execute, daemon=True).start()
|
||||
|
||||
interrupted = False
|
||||
while not done.wait(timeout=0.05):
|
||||
if goal_handle.is_cancel_requested:
|
||||
if (goal_handle.is_cancel_requested or generation != self._motion_serial.generation()
|
||||
or lease.lost):
|
||||
interrupted = True
|
||||
# Repeat to cover stop racing the blocking execute() submission.
|
||||
try:
|
||||
self._moveit.get_trajectory_execution_manager().stop_execution()
|
||||
except Exception:
|
||||
pass
|
||||
done.wait()
|
||||
return None, "canceled"
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Stop retry failed: {exc}")
|
||||
if goal_handle.is_cancel_requested:
|
||||
return None, "canceled"
|
||||
self._check_operation()
|
||||
if interrupted:
|
||||
return False, "Выполнение траектории остановлено"
|
||||
|
||||
if failed.is_set():
|
||||
return False, "Выполнение траектории завершилось ошибкой"
|
||||
@@ -165,8 +238,7 @@ class IiwaMotionServer(Node):
|
||||
result.success = False
|
||||
result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
|
||||
self.get_logger().error(result.message)
|
||||
goal_handle.abort()
|
||||
return result
|
||||
return result, False, result.message
|
||||
|
||||
pipeline, planner_id, plan_time = PLANNERS[planner_key]
|
||||
|
||||
@@ -198,7 +270,7 @@ class IiwaMotionServer(Node):
|
||||
|
||||
plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
|
||||
ok, msg = self._plan_and_execute(plan_params, goal_handle)
|
||||
return self._finish_action(goal_handle, result, ok, msg)
|
||||
return result, ok, msg
|
||||
|
||||
def _execute_joints(self, goal_handle):
|
||||
req = goal_handle.request
|
||||
@@ -230,7 +302,7 @@ class IiwaMotionServer(Node):
|
||||
"ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
|
||||
)
|
||||
ok, msg = self._plan_and_execute(plan_params, goal_handle)
|
||||
return self._finish_action(goal_handle, result, ok, msg)
|
||||
return result, ok, msg
|
||||
|
||||
def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
|
||||
name = request.name.strip()
|
||||
@@ -254,30 +326,26 @@ class IiwaMotionServer(Node):
|
||||
plan_params = self._make_plan_params(
|
||||
"pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
|
||||
)
|
||||
plan_result = self._arm.plan(single_plan_parameters=plan_params)
|
||||
if not plan_result:
|
||||
response.success = False
|
||||
response.message = f"Не удалось построить траекторию для '{name}'"
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
class NamedGoal:
|
||||
is_cancel_requested = False
|
||||
|
||||
exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
|
||||
if exec_result:
|
||||
response.success = True
|
||||
response.message = f"Переместился в '{name}'"
|
||||
self.get_logger().info(response.message)
|
||||
else:
|
||||
response.success = False
|
||||
response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
|
||||
self.get_logger().error(response.message)
|
||||
ok, message = self._plan_and_execute(plan_params, NamedGoal())
|
||||
response.success = bool(ok)
|
||||
response.message = message
|
||||
return response
|
||||
|
||||
def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
|
||||
try:
|
||||
self._moveit.get_trajectory_execution_manager().stop_execution()
|
||||
with self._lease():
|
||||
self._stop_execution()
|
||||
response.success = True
|
||||
response.message = "Выполнение траектории остановлено"
|
||||
except Exception as exc:
|
||||
# Failure to reserve must never prevent the requested high-priority stop.
|
||||
try:
|
||||
self._stop_execution()
|
||||
except Exception as stop_exc:
|
||||
self.get_logger().error(f"Stop failed: {stop_exc}")
|
||||
response.success = False
|
||||
response.message = str(exc)
|
||||
self.get_logger().info(f"[stop] {response.message}")
|
||||
@@ -294,7 +362,10 @@ def main(args=None):
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
if "node" in locals():
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -0,0 +1,471 @@
|
||||
#include <iiwa_planning/tcp_gizmo_policy.hpp>
|
||||
#include <iiwa_msgs/msg/gizmo_command.hpp>
|
||||
#include <iiwa_msgs/msg/gizmo_state.hpp>
|
||||
#include <iiwa_msgs/srv/motion_priority.hpp>
|
||||
#include <control_msgs/action/follow_joint_trajectory.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <moveit_servo/servo.hpp>
|
||||
#include <moveit_servo/utils/common.hpp>
|
||||
#include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <trajectory_msgs/msg/joint_trajectory.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <deque>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace iiwa_planning {
|
||||
class TcpGizmo {
|
||||
using Trajectory = trajectory_msgs::msg::JointTrajectory;
|
||||
using Priority = iiwa_msgs::srv::MotionPriority;
|
||||
using Follow = control_msgs::action::FollowJointTrajectory;
|
||||
using ClientGoal = rclcpp_action::ClientGoalHandle<Follow>;
|
||||
using ServerGoal = rclcpp_action::ServerGoalHandle<Follow>;
|
||||
struct ProxyGoal {
|
||||
std::shared_ptr<ServerGoal> upstream;
|
||||
ClientGoal::SharedPtr downstream;
|
||||
bool cancel_requested{false};
|
||||
bool cancel_sent{false};
|
||||
bool finished{false};
|
||||
};
|
||||
public:
|
||||
explicit TcpGizmo(const rclcpp::Node::SharedPtr& node) : node_(node) {
|
||||
if (!param<bool>("enabled", false) || !param<bool>("simulation", false))
|
||||
throw std::runtime_error("TCP gizmo requires enabled=true and simulation=true");
|
||||
group_name_ = param<std::string>("planning_group", "iiwa_arm");
|
||||
tcp_ = param<std::string>("pose_link", "tcp");
|
||||
frame_ = param<std::string>("default_frame", "base_link");
|
||||
period_ = positive("publish_period", .032);
|
||||
command_timeout_ = positive("command_timeout", .3);
|
||||
state_timeout_ = positive("state_timeout", .5);
|
||||
linear_speed_ = positive("max_linear_speed", .10);
|
||||
angular_speed_ = positive("max_angular_speed", .4);
|
||||
listener_ = std::make_shared<servo::ParamListener>(node_, "moveit_servo");
|
||||
const auto params = listener_->get_params();
|
||||
if (params.move_group_name != group_name_ || std::abs(params.publish_period - period_) > 1e-9)
|
||||
throw std::runtime_error("Servo group/period must match TCP gizmo parameters");
|
||||
scene_ = moveit_servo::createPlanningSceneMonitor(node_, params);
|
||||
if (!scene_ || !scene_->getRobotModel()) throw std::runtime_error("Missing planning scene");
|
||||
group_ = scene_->getRobotModel()->getJointModelGroup(group_name_);
|
||||
if (!group_ || !group_->getSolverInstance()) throw std::runtime_error("Missing IK solver");
|
||||
tip_ = group_->getSolverInstance()->getTipFrame();
|
||||
const auto model = scene_->getRobotModel();
|
||||
if (!model->hasLinkModel(tcp_) || !model->hasLinkModel(frame_) || !model->hasLinkModel(tip_))
|
||||
throw std::runtime_error("Unknown TCP, frame or solver tip");
|
||||
if (moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tcp_)) !=
|
||||
moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tip_)))
|
||||
throw std::runtime_error("TCP must be rigidly attached to the IK solver tip");
|
||||
output_ = node_->create_publisher<Trajectory>("/cobot/tcp_gizmo/controller_command", 10);
|
||||
state_pub_ = node_->create_publisher<iiwa_msgs::msg::GizmoState>("/cobot/tcp_gizmo/state", 1);
|
||||
command_sub_ = node_->create_subscription<iiwa_msgs::msg::GizmoCommand>(
|
||||
"/cobot/tcp_gizmo/command", 1, [this](const iiwa_msgs::msg::GizmoCommand& msg) {
|
||||
try { command(msg); }
|
||||
catch (const std::exception& e) { reject(std::string("target error: ") + e.what()); }
|
||||
});
|
||||
action_client_ = rclcpp_action::create_client<Follow>(node_, "/cobot/tcp_gizmo/follow_joint_trajectory");
|
||||
action_server_ = rclcpp_action::create_server<Follow>(node_, "/iiwa_arm_controller/follow_joint_trajectory",
|
||||
[this](const rclcpp_action::GoalUUID&, std::shared_ptr<const Follow::Goal>) {
|
||||
if (!action_client_->action_server_is_ready()) return rclcpp_action::GoalResponse::REJECT;
|
||||
++high_goals_count_;
|
||||
gate_.action(true); // Block low before accepting a public high goal.
|
||||
reason_ = "trajectory action";
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
},
|
||||
[this](const std::shared_ptr<ServerGoal> goal) {
|
||||
const auto it = high_goals_.find(goal->get_goal_id());
|
||||
if (it != high_goals_.end()) {
|
||||
it->second->cancel_requested = true;
|
||||
cancelHigh(it->second);
|
||||
}
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[this](const std::shared_ptr<ServerGoal> goal) {
|
||||
auto proxy = std::make_shared<ProxyGoal>();
|
||||
proxy->upstream = goal;
|
||||
high_goals_[goal->get_goal_id()] = proxy;
|
||||
after_low_.push_back([this, proxy]() { sendHigh(proxy); });
|
||||
flushHigh();
|
||||
});
|
||||
normal_sub_ = node_->create_subscription<Trajectory>("/iiwa_arm_controller/joint_trajectory", 10,
|
||||
[this](const Trajectory& msg) {
|
||||
// The low action must have a terminal controller result before ANY high
|
||||
// output. DDS publication/cancel acknowledgements alone are insufficient.
|
||||
gate_.normal(trajectoryDeadline(msg));
|
||||
reason_ = "ordinary trajectory";
|
||||
after_low_.push_back([this, msg]() {
|
||||
gate_.normal(trajectoryDeadline(msg));
|
||||
output_->publish(msg);
|
||||
});
|
||||
flushHigh();
|
||||
});
|
||||
priority_ = node_->create_service<Priority>("/cobot/tcp_gizmo/priority",
|
||||
[this](const std::shared_ptr<rmw_request_id_t> header, const Priority::Request::SharedPtr request) {
|
||||
const auto token = ++lease_requests_[request->owner];
|
||||
if (!request->acquire) {
|
||||
gate_.release(request->owner);
|
||||
replyPriority(header, true, "priority released");
|
||||
return;
|
||||
}
|
||||
if (!gate_.acquire(request->owner, request->ttl, steady())) {
|
||||
replyPriority(header, false, "owner and finite TTL in (0, 60] required");
|
||||
return;
|
||||
}
|
||||
reason_ = "waiting for low action to stop";
|
||||
after_low_.push_back([this, header, request, token]() {
|
||||
if (lease_requests_[request->owner] != token) {
|
||||
replyPriority(header, false, "priority request superseded"); return;
|
||||
}
|
||||
// TTL starts at acknowledgement, even if controller completion was
|
||||
// delayed. The pending callback itself prevents any new low command.
|
||||
gate_.acquire(request->owner, request->ttl, steady());
|
||||
replyPriority(header, true, "controller low action stopped; priority granted");
|
||||
reason_ = "motion priority lease";
|
||||
});
|
||||
flushHigh();
|
||||
});
|
||||
timer_ = node_->create_wall_timer(std::chrono::duration<double>(period_), [this]() { tick(); });
|
||||
}
|
||||
|
||||
private:
|
||||
template<class T> T param(const std::string& name, const T& value) {
|
||||
if (!node_->has_parameter(name)) return node_->declare_parameter<T>(name, value);
|
||||
return node_->get_parameter(name).get_value<T>();
|
||||
}
|
||||
double positive(const std::string& name, double value) {
|
||||
value = param<double>(name, value);
|
||||
if (!std::isfinite(value) || value <= 0.) throw std::runtime_error("Invalid " + name);
|
||||
return value;
|
||||
}
|
||||
static double steady() {
|
||||
return std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
moveit::core::RobotStatePtr current() {
|
||||
auto monitor = scene_->getStateMonitor();
|
||||
if (!monitor || !monitor->haveCompleteState(rclcpp::Duration::from_seconds(state_timeout_))) return {};
|
||||
auto pair = monitor->getCurrentStateAndTime();
|
||||
const double age = (node_->now() - pair.second).seconds();
|
||||
if (age < -.01 || age > state_timeout_) return {};
|
||||
const auto stamp = pair.second.nanoseconds();
|
||||
if (stamp != feedback_stamp_) { feedback_stamp_ = stamp; feedback_received_ = steady(); }
|
||||
if (steady() - feedback_received_ > state_timeout_) return {};
|
||||
pair.first->update();
|
||||
std::vector<double> positions;
|
||||
pair.first->copyJointGroupPositions(group_, positions);
|
||||
if (!std::all_of(positions.begin(), positions.end(), [](double x) { return std::isfinite(x); })) return {};
|
||||
return pair.first;
|
||||
}
|
||||
bool settled(const moveit::core::RobotState& state) {
|
||||
std::vector<double> positions;
|
||||
state.copyJointGroupPositions(group_, positions);
|
||||
const double now = node_->now().seconds();
|
||||
if (previous_.size() != positions.size() || now < previous_time_) {
|
||||
previous_ = positions; previous_time_ = now; stable_since_ = now; return false;
|
||||
}
|
||||
if (now > previous_time_) {
|
||||
double delta = 0.;
|
||||
for (std::size_t i = 0; i < positions.size(); ++i) delta = std::max(delta, std::abs(positions[i] - previous_[i]));
|
||||
if (delta / (now - previous_time_) > .01) stable_since_ = now;
|
||||
previous_ = positions; previous_time_ = now;
|
||||
}
|
||||
return now - stable_since_ >= .15;
|
||||
}
|
||||
double trajectoryDeadline(const Trajectory& msg) const {
|
||||
double start = node_->now().seconds();
|
||||
if (rclcpp::Time(msg.header.stamp).nanoseconds() != 0)
|
||||
start = std::max(start, rclcpp::Time(msg.header.stamp).seconds());
|
||||
double duration = 0.;
|
||||
for (const auto& point : msg.points)
|
||||
duration = std::max(duration, rclcpp::Duration(point.time_from_start).seconds());
|
||||
return start + duration;
|
||||
}
|
||||
void replyPriority(const std::shared_ptr<rmw_request_id_t>& header, bool success, const std::string& message) {
|
||||
Priority::Response response;
|
||||
response.success = success; response.message = message;
|
||||
priority_->send_response(*header, response);
|
||||
}
|
||||
void flushHigh() {
|
||||
if (!low_barrier_.idle()) return;
|
||||
while (!after_low_.empty()) {
|
||||
auto callback = std::move(after_low_.front());
|
||||
after_low_.pop_front();
|
||||
callback();
|
||||
}
|
||||
}
|
||||
void sendLow(const Trajectory& trajectory) {
|
||||
if (!action_client_->action_server_is_ready()) { reject("controller action unavailable"); return; }
|
||||
if (!low_barrier_.begin()) return;
|
||||
Follow::Goal goal;
|
||||
goal.trajectory = trajectory;
|
||||
// One released marker pose is one complete trajectory. The user chose
|
||||
// to finish this trajectory before dispatching a queued ordinary command.
|
||||
// No low CancelGoal request is needed (including the JTC cancel/result race).
|
||||
goal.goal_time_tolerance = rclcpp::Duration::from_seconds(state_timeout_);
|
||||
rclcpp_action::Client<Follow>::SendGoalOptions options;
|
||||
options.goal_response_callback = [this](const ClientGoal::SharedPtr handle) {
|
||||
low_goal_ = handle;
|
||||
low_barrier_.accepted(static_cast<bool>(handle));
|
||||
if (!handle) {
|
||||
gate_.invalidate(); reason_ = "controller rejected low trajectory"; flushHigh();
|
||||
}
|
||||
};
|
||||
options.result_callback = [this](const ClientGoal::WrappedResult& result) {
|
||||
low_goal_.reset(); low_barrier_.terminal();
|
||||
// A new epoch acknowledges completion to the marker and permits a new
|
||||
// gesture. Never redispatch the same goal after a successful result.
|
||||
gate_.invalidate();
|
||||
reason_ = result.code == rclcpp_action::ResultCode::SUCCEEDED ?
|
||||
"target reached" : "controller did not complete TCP trajectory";
|
||||
flushHigh();
|
||||
};
|
||||
try { action_client_->async_send_goal(goal, options); }
|
||||
catch (const std::exception& error) {
|
||||
// An exception leaves delivery uncertain. Fail closed: do not forward a
|
||||
// high command without a terminal controller result for this send.
|
||||
gate_.invalidate(); reason_ = std::string("low action send uncertain: ") + error.what();
|
||||
}
|
||||
}
|
||||
void cancelHigh(const std::shared_ptr<ProxyGoal>& proxy) {
|
||||
if (proxy->downstream && proxy->cancel_requested && !proxy->cancel_sent && !proxy->finished) {
|
||||
proxy->cancel_sent = true;
|
||||
action_client_->async_cancel_goal(proxy->downstream);
|
||||
}
|
||||
}
|
||||
void finishHigh(const std::shared_ptr<ProxyGoal>& proxy, rclcpp_action::ResultCode code,
|
||||
const std::shared_ptr<Follow::Result>& result) {
|
||||
if (proxy->finished) return;
|
||||
proxy->finished = true;
|
||||
if (proxy->upstream->is_canceling()) proxy->upstream->canceled(result);
|
||||
else if (code == rclcpp_action::ResultCode::SUCCEEDED) proxy->upstream->succeed(result);
|
||||
else proxy->upstream->abort(result);
|
||||
high_goals_.erase(proxy->upstream->get_goal_id());
|
||||
if (high_goals_count_ > 0) --high_goals_count_;
|
||||
gate_.action(high_goals_count_ > 0);
|
||||
}
|
||||
void sendHigh(const std::shared_ptr<ProxyGoal>& proxy) {
|
||||
if (proxy->cancel_requested || proxy->upstream->is_canceling()) {
|
||||
auto result = std::make_shared<Follow::Result>();
|
||||
result->error_string = "canceled before controller dispatch";
|
||||
finishHigh(proxy, rclcpp_action::ResultCode::CANCELED, result); return;
|
||||
}
|
||||
if (!action_client_->action_server_is_ready()) {
|
||||
auto result = std::make_shared<Follow::Result>();
|
||||
result->error_code = Follow::Result::INVALID_GOAL;
|
||||
result->error_string = "controller action unavailable";
|
||||
finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result); return;
|
||||
}
|
||||
rclcpp_action::Client<Follow>::SendGoalOptions options;
|
||||
options.goal_response_callback = [this, proxy](const ClientGoal::SharedPtr handle) {
|
||||
proxy->downstream = handle;
|
||||
if (!handle) {
|
||||
auto result = std::make_shared<Follow::Result>();
|
||||
result->error_code = Follow::Result::INVALID_GOAL;
|
||||
result->error_string = "controller rejected trajectory";
|
||||
finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result);
|
||||
} else cancelHigh(proxy);
|
||||
};
|
||||
options.feedback_callback = [proxy](ClientGoal::SharedPtr, const std::shared_ptr<const Follow::Feedback> feedback) {
|
||||
if (!proxy->finished && proxy->upstream->is_active()) proxy->upstream->publish_feedback(std::make_shared<Follow::Feedback>(*feedback));
|
||||
};
|
||||
options.result_callback = [this, proxy](const ClientGoal::WrappedResult& result) {
|
||||
finishHigh(proxy, result.code, result.result);
|
||||
};
|
||||
try { action_client_->async_send_goal(*proxy->upstream->get_goal(), options); }
|
||||
catch (const std::exception& error) {
|
||||
// Keep its high reservation on ambiguous delivery; exposing low motion
|
||||
// here could overwrite a goal that the controller actually received.
|
||||
reason_ = std::string("high action send uncertain: ") + error.what();
|
||||
}
|
||||
}
|
||||
void reject(const std::string& reason) {
|
||||
RCLCPP_WARN(node_->get_logger(), "TCP gizmo rejected target: %s", reason.c_str());
|
||||
gate_.invalidate(); reason_ = reason;
|
||||
}
|
||||
bool safe(moveit::core::RobotState& state, const planning_scene::PlanningSceneConstPtr& scene) {
|
||||
state.update();
|
||||
return state.satisfiesBounds(group_) && !scene->isStateColliding(state, group_name_);
|
||||
}
|
||||
void command(const iiwa_msgs::msg::GizmoCommand& msg) {
|
||||
auto state = current();
|
||||
if (!state) { reject("joint state unavailable or stale"); return; }
|
||||
if (!after_low_.empty() || !gate_.allowed(steady(), node_->now().seconds(), settled(*state))) return;
|
||||
if (msg.epoch != gate_.epoch()) return;
|
||||
if (!low_barrier_.idle()) return;
|
||||
const auto& p = msg.target.pose;
|
||||
const double age = (node_->now() - rclcpp::Time(msg.target.header.stamp)).seconds();
|
||||
Eigen::Quaterniond q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
|
||||
Eigen::Vector3d t(p.position.x, p.position.y, p.position.z);
|
||||
if (msg.target.header.frame_id != frame_ || !q.coeffs().allFinite() || !t.allFinite() ||
|
||||
std::abs(q.norm() - 1.) > .01 || age < -.01 || age > command_timeout_) {
|
||||
reject("invalid or stale target"); return;
|
||||
}
|
||||
Eigen::Isometry3d requested = Eigen::Isometry3d::Identity();
|
||||
requested.linear() = q.normalized().toRotationMatrix(); requested.translation() = t;
|
||||
const auto actual = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
|
||||
if (!requiresGizmoMotion((requested.translation() - actual.translation()).norm(),
|
||||
Eigen::Quaterniond(requested.linear()).angularDistance(Eigen::Quaterniond(actual.linear())))) {
|
||||
gate_.invalidate();
|
||||
reason_ = "target reached"; return;
|
||||
}
|
||||
const auto model_target = state->getGlobalLinkTransform(frame_) * requested;
|
||||
// Solve once on release. setFromIK handles the tool's fixed TCP offset.
|
||||
moveit::core::RobotState candidate(*state);
|
||||
planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
|
||||
const bool valid = candidate.setFromIK(group_, model_target, tcp_, .01,
|
||||
[this, &scene](moveit::core::RobotState* s, const moveit::core::JointModelGroup* group, const double* values) {
|
||||
s->setJointGroupPositions(group, values); return safe(*s, scene);
|
||||
});
|
||||
if (!valid || !safe(candidate, scene)) { reject("target unreachable, outside limits or colliding"); return; }
|
||||
// Do not accept approximate IK solutions as reachable targets.
|
||||
const auto reached = candidate.getGlobalLinkTransform(tcp_);
|
||||
if ((reached.translation() - model_target.translation()).norm() > .002 ||
|
||||
Eigen::AngleAxisd(reached.linear().transpose() * model_target.linear()).angle() > .01) {
|
||||
reject("IK solution does not reach target"); return;
|
||||
}
|
||||
candidate.copyJointGroupPositions(group_, ik_target_);
|
||||
gate_.accept(msg.epoch, steady(), command_timeout_);
|
||||
reason_ = "tracking";
|
||||
}
|
||||
void tick() {
|
||||
flushHigh();
|
||||
auto state = current();
|
||||
if (!state) {
|
||||
if (gate_.hasTarget()) reject("joint state unavailable or stale");
|
||||
publishState({}, false, "joint state unavailable or stale"); return;
|
||||
}
|
||||
const bool available = after_low_.empty() && gate_.allowed(steady(), node_->now().seconds(), settled(*state));
|
||||
if (!available) {
|
||||
publishState(state, false, reason_); return;
|
||||
}
|
||||
// command_timeout validates the age at receipt. A released goal is a
|
||||
// committed action and needs no stream of keepalive mouse commands.
|
||||
if (gate_.hasTarget() && low_barrier_.idle()) {
|
||||
try { step(state); }
|
||||
catch (const std::exception& e) { reject(std::string("TCP trajectory error: ") + e.what()); }
|
||||
}
|
||||
publishState(state, true, reason_);
|
||||
}
|
||||
void step(const moveit::core::RobotStatePtr& state) {
|
||||
const auto& names = group_->getVariableNames();
|
||||
if (ik_target_.size() != names.size()) { reject("missing IK target"); return; }
|
||||
std::vector<double> actual;
|
||||
state->copyJointGroupPositions(group_, actual);
|
||||
Eigen::VectorXd delta(names.size());
|
||||
double duration = 2. * period_;
|
||||
double max_travel = 0.;
|
||||
for (std::size_t i = 0; i < names.size(); ++i) {
|
||||
delta[i] = ik_target_[i] - actual[i];
|
||||
const double travel = std::abs(delta[i]);
|
||||
max_travel = std::max(max_travel, travel);
|
||||
const auto& limit = state->getRobotModel()->getVariableBounds(names[i]);
|
||||
// q(u) = q0 + (3u^2 - 2u^3) * delta, with zero endpoint velocities.
|
||||
if (limit.velocity_bounded_)
|
||||
duration = std::max(duration, 1.5 * travel / limit.max_velocity_);
|
||||
if (limit.acceleration_bounded_)
|
||||
duration = std::max(duration, std::sqrt(6. * travel / limit.max_acceleration_));
|
||||
}
|
||||
if (!delta.allFinite() || !std::isfinite(duration)) { reject("invalid trajectory limits"); return; }
|
||||
|
||||
// Validate the complete path, including the actual tool tip. Use forward
|
||||
// Jacobians only to measure Cartesian speed; no inverse/singularity gate.
|
||||
// Densely sample the cubic path and increase duration to obey BOTH TCP
|
||||
// translation and rotation limits throughout it, not just at its endpoints.
|
||||
const Eigen::Vector3d tcp_offset = (state->getGlobalLinkTransform(tip_).inverse() *
|
||||
state->getGlobalLinkTransform(tcp_)).translation();
|
||||
const auto* tip_link = state->getRobotModel()->getLinkModel(tip_);
|
||||
const int samples = std::max(100, static_cast<int>(std::ceil(max_travel / .01)));
|
||||
planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
|
||||
moveit::core::RobotState sample(*state);
|
||||
Eigen::MatrixXd jacobian;
|
||||
std::vector<double> positions(actual.size());
|
||||
for (int k = 0; k <= samples; ++k) {
|
||||
const double u = static_cast<double>(k) / samples;
|
||||
const double blend = u * u * (3. - 2. * u);
|
||||
for (std::size_t i = 0; i < positions.size(); ++i) positions[i] = actual[i] + blend * delta[i];
|
||||
sample.setJointGroupPositions(group_, positions);
|
||||
if (!safe(sample, scene)) { reject("TCP trajectory is outside limits or colliding"); return; }
|
||||
if (!sample.getJacobian(group_, tip_link, tcp_offset, jacobian)) {
|
||||
reject("cannot compute tool TCP speed"); return;
|
||||
}
|
||||
const Eigen::VectorXd twist = jacobian * delta * (6. * u * (1. - u));
|
||||
duration = std::max(duration, 1.01 * twist.head<3>().norm() / linear_speed_);
|
||||
duration = std::max(duration, 1.01 * twist.tail<3>().norm() / angular_speed_);
|
||||
}
|
||||
if (!std::isfinite(duration)) { reject("invalid TCP trajectory duration"); return; }
|
||||
Trajectory message;
|
||||
message.joint_names = names;
|
||||
trajectory_msgs::msg::JointTrajectoryPoint start, end;
|
||||
start.positions = actual;
|
||||
start.velocities.assign(actual.size(), 0.);
|
||||
start.time_from_start = rclcpp::Duration::from_seconds(.001);
|
||||
end.positions = ik_target_;
|
||||
end.velocities.assign(actual.size(), 0.);
|
||||
end.time_from_start = rclcpp::Duration::from_seconds(duration + .001);
|
||||
message.points = {start, end};
|
||||
reason_ = "executing released TCP target";
|
||||
sendLow(message);
|
||||
}
|
||||
void publishState(const moveit::core::RobotStatePtr& state, bool enabled, const std::string& reason) {
|
||||
iiwa_msgs::msg::GizmoState message;
|
||||
message.actual.header.frame_id = (state || have_actual_) ? frame_ : "";
|
||||
message.actual.header.stamp = node_->now();
|
||||
message.actual.pose = last_actual_;
|
||||
if (state) {
|
||||
const auto pose = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
|
||||
message.actual.pose.position.x = pose.translation().x(); message.actual.pose.position.y = pose.translation().y();
|
||||
message.actual.pose.position.z = pose.translation().z();
|
||||
const Eigen::Quaterniond q(pose.linear());
|
||||
message.actual.pose.orientation.x = q.x(); message.actual.pose.orientation.y = q.y();
|
||||
message.actual.pose.orientation.z = q.z(); message.actual.pose.orientation.w = q.w();
|
||||
last_actual_ = message.actual.pose; have_actual_ = true;
|
||||
}
|
||||
message.enabled = enabled; message.epoch = gate_.epoch(); message.reason = reason;
|
||||
state_pub_->publish(message);
|
||||
}
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
planning_scene_monitor::PlanningSceneMonitorPtr scene_;
|
||||
std::shared_ptr<servo::ParamListener> listener_;
|
||||
const moveit::core::JointModelGroup* group_{};
|
||||
std::string group_name_, tcp_, tip_, frame_, reason_{"ready"};
|
||||
double period_{}, command_timeout_{}, state_timeout_{}, linear_speed_{}, angular_speed_{};
|
||||
GizmoPolicy gate_;
|
||||
std::vector<double> ik_target_;
|
||||
bool have_actual_{false};
|
||||
LowGoalBarrier low_barrier_;
|
||||
ClientGoal::SharedPtr low_goal_;
|
||||
std::deque<std::function<void()>> after_low_;
|
||||
std::map<std::string, std::uint64_t> lease_requests_;
|
||||
std::map<rclcpp_action::GoalUUID, std::shared_ptr<ProxyGoal>> high_goals_;
|
||||
std::size_t high_goals_count_{0};
|
||||
rclcpp_action::Client<Follow>::SharedPtr action_client_;
|
||||
rclcpp_action::Server<Follow>::SharedPtr action_server_;
|
||||
std::vector<double> previous_;
|
||||
double previous_time_{0.}, stable_since_{0.}, feedback_received_{0.};
|
||||
std::int64_t feedback_stamp_{-1};
|
||||
geometry_msgs::msg::Pose last_actual_{};
|
||||
rclcpp::Publisher<Trajectory>::SharedPtr output_;
|
||||
rclcpp::Publisher<iiwa_msgs::msg::GizmoState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<iiwa_msgs::msg::GizmoCommand>::SharedPtr command_sub_;
|
||||
rclcpp::Subscription<Trajectory>::SharedPtr normal_sub_;
|
||||
rclcpp::Service<Priority>::SharedPtr priority_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<rclcpp::Node>("tcp_gizmo", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
|
||||
try {
|
||||
iiwa_planning::TcpGizmo gizmo(node);
|
||||
// All motion, priority, target and timer callbacks are serialized.
|
||||
rclcpp::executors::SingleThreadedExecutor executor;
|
||||
executor.add_node(node); executor.spin();
|
||||
} catch (const std::exception& error) {
|
||||
RCLCPP_FATAL(node->get_logger(), "%s", error.what()); rclcpp::shutdown(); return 1;
|
||||
}
|
||||
rclcpp::shutdown(); return 0;
|
||||
}
|
||||
@@ -0,0 +1,232 @@
|
||||
"""Manual regression against Webots: release commits exactly one TCP action.
|
||||
|
||||
Exercises the marker model with real ROS/controller feedback. Physical mouse
|
||||
input is covered separately by the Webots plugin adapter tests.
|
||||
"""
|
||||
import copy
|
||||
import threading
|
||||
import time
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.parameter_client import AsyncParameterClient
|
||||
from rclpy.qos import QoSProfile, DurabilityPolicy
|
||||
from action_msgs.msg import GoalStatusArray
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from controller_manager_msgs.srv import ListControllers
|
||||
from trajectory_msgs.msg import JointTrajectoryPoint
|
||||
from iiwa_msgs.msg import GizmoCommand, GizmoState
|
||||
from iiwa_utils.tcp_gizmo import pose_matrix
|
||||
from iiwa_utils.tcp_gizmo_model import DragTarget
|
||||
from iiwa_planning.motion_priority import MotionPriorityClient
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = rclpy.create_node('gizmo_webots_regression')
|
||||
state = {}
|
||||
drag = DragTarget()
|
||||
|
||||
def capture(msg):
|
||||
state['gizmo'] = msg
|
||||
if msg.actual.header.frame_id:
|
||||
drag.update_feedback(pose_matrix(msg.actual.pose), msg.enabled, msg.epoch)
|
||||
|
||||
node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', capture, 10)
|
||||
node.create_subscription(
|
||||
GoalStatusArray, '/cobot/tcp_gizmo/follow_joint_trajectory/_action/status',
|
||||
lambda msg: state.update(status=msg),
|
||||
QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
|
||||
publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
|
||||
|
||||
def pump(duration):
|
||||
end = time.monotonic() + duration
|
||||
while time.monotonic() < end:
|
||||
rclpy.spin_once(node, timeout_sec=.01)
|
||||
|
||||
def wait(predicate, timeout=30.):
|
||||
end = time.monotonic() + timeout
|
||||
while not predicate():
|
||||
assert time.monotonic() < end, f'Timeout: {state.get("gizmo")}'
|
||||
rclpy.spin_once(node, timeout_sec=.01)
|
||||
|
||||
def ids():
|
||||
return {bytes(g.goal_info.goal_id.uuid) for g in state['status'].status_list} if 'status' in state else set()
|
||||
|
||||
def error(goal):
|
||||
diff = np.linalg.inv(pose_matrix(goal.pose)) @ pose_matrix(state['gizmo'].actual.pose)
|
||||
return np.linalg.norm(diff[:3, 3]), Rotation.from_matrix(diff[:3, :3]).magnitude()
|
||||
|
||||
def release(goal):
|
||||
initial = copy.deepcopy(state['gizmo'].actual)
|
||||
epoch = state['gizmo'].epoch
|
||||
before = ids()
|
||||
drag.step(pose_matrix(initial.pose), False)
|
||||
for _ in range(8):
|
||||
shown, command = drag.step(pose_matrix(goal.pose), True)
|
||||
assert command is None, 'Mouse hold must only edit the preview'
|
||||
pump(.02)
|
||||
assert ids() == before, 'Controller received an action before release'
|
||||
assert error(initial)[0] < .001, 'Robot moved while mouse was held'
|
||||
shown, command = drag.step(shown, False)
|
||||
assert command is not None
|
||||
msg = GizmoCommand(target=copy.deepcopy(goal), epoch=epoch)
|
||||
msg.target.header.stamp = state['gizmo'].actual.header.stamp
|
||||
publisher.publish(msg)
|
||||
return epoch, before
|
||||
|
||||
def finish(goal, epoch, before):
|
||||
# No command keepalive: one release must complete even beyond timeout.
|
||||
wait(lambda: state['gizmo'].epoch != epoch)
|
||||
assert state['gizmo'].reason == 'target reached', state['gizmo'].reason
|
||||
wait(lambda: len(ids() - before) == 1, 2.)
|
||||
wait(lambda: error(goal)[0] < .002 and error(goal)[1] < .01, 2.)
|
||||
pump(.15)
|
||||
assert len(ids() - before) == 1, 'A release generated repeated actions'
|
||||
assert not any(g.status in (1, 2, 3) for g in state['status'].status_list)
|
||||
shown, command = drag.step(pose_matrix(goal.pose), False)
|
||||
assert command is None
|
||||
np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
|
||||
|
||||
try:
|
||||
params = AsyncParameterClient(node, '/tcp_gizmo')
|
||||
assert params.wait_for_services(timeout_sec=30.)
|
||||
future = params.get_parameters(['simulation', 'use_sim_time', 'pose_link', 'robot_description_semantic',
|
||||
'max_linear_speed'])
|
||||
wait(future.done)
|
||||
values = future.result().values
|
||||
assert values[0].bool_value and values[1].bool_value, 'Simulation only'
|
||||
assert values[2].string_value == 'tcp', 'Gizmo must use active tool TCP'
|
||||
srdf = ET.fromstring(values[3].string_value)
|
||||
controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
|
||||
assert controllers.wait_for_service(timeout_sec=30.)
|
||||
deadline = time.monotonic() + 30.
|
||||
while True:
|
||||
listing = controllers.call_async(ListControllers.Request())
|
||||
wait(listing.done)
|
||||
if any(c.name == 'iiwa_arm_controller' and c.state == 'active' for c in listing.result().controller):
|
||||
break
|
||||
assert time.monotonic() < deadline
|
||||
pump(.1)
|
||||
wait(lambda: 'gizmo' in state and state['gizmo'].enabled)
|
||||
action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
|
||||
assert action.wait_for_server(timeout_sec=5.)
|
||||
|
||||
def ordinary(name):
|
||||
joints = srdf.find(f"group_state[@name='{name}']")
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory.joint_names = [j.attrib['name'] for j in joints]
|
||||
point = JointTrajectoryPoint(positions=[float(j.attrib['value']) for j in joints])
|
||||
point.time_from_start.sec = 3
|
||||
goal.trajectory.points = [point]
|
||||
future = action.send_goal_async(goal)
|
||||
wait(future.done)
|
||||
assert future.result().accepted
|
||||
return future.result().get_result_async()
|
||||
|
||||
done = ordinary('home')
|
||||
wait(done.done)
|
||||
assert done.result().result.error_code == 0
|
||||
wait(lambda: state['gizmo'].enabled)
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
goal.pose.position.z -= .02
|
||||
finish(goal, *release(goal))
|
||||
print('PASS: straight posture -> 2 cm descent; no action during drag, exactly one on release', flush=True)
|
||||
|
||||
done = ordinary('work')
|
||||
wait(done.done)
|
||||
assert done.result().result.error_code == 0
|
||||
wait(lambda: state['gizmo'].enabled)
|
||||
for kind in ('translation', 'rotation'):
|
||||
for axis in range(3):
|
||||
for sign in (1., -1.):
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
if kind == 'translation':
|
||||
key = ('x', 'y', 'z')[axis]
|
||||
setattr(goal.pose.position, key, getattr(goal.pose.position, key) + sign * .01)
|
||||
else:
|
||||
rotvec = np.zeros(3)
|
||||
rotvec[axis] = sign * .03
|
||||
q = (Rotation.from_rotvec(rotvec) * Rotation.from_matrix(pose_matrix(goal.pose)[:3, :3])).as_quat()
|
||||
goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = q.tolist()
|
||||
finish(goal, *release(goal))
|
||||
print(f'PASS: {kind} axis={axis} sign={sign:+.0f}; exactly one action', flush=True)
|
||||
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
goal.pose.position.x = 10.
|
||||
epoch, before = release(goal)
|
||||
wait(lambda: state['gizmo'].epoch != epoch)
|
||||
assert 'unreachable' in state['gizmo'].reason
|
||||
assert ids() == before, 'Invalid target dispatched an action'
|
||||
shown, command = drag.step(pose_matrix(goal.pose), False)
|
||||
np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
|
||||
assert command is None
|
||||
print('PASS: unreachable released target resets marker; no controller action', flush=True)
|
||||
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
goal.pose.position.x -= .1
|
||||
epoch, before = release(goal)
|
||||
wait(lambda: len(ids() - before) == 1)
|
||||
low_id = next(iter(ids() - before))
|
||||
assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 2 for g in state['status'].status_list)
|
||||
done = ordinary('work')
|
||||
# The high request may be accepted upstream, but must not be dispatched
|
||||
# to JTC until the complete gizmo action has succeeded.
|
||||
deadline = time.monotonic() + 20.
|
||||
while not done.done():
|
||||
assert time.monotonic() < deadline, 'Queued ordinary command did not complete'
|
||||
pump(.01)
|
||||
if len(ids() - before) > 1:
|
||||
assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
|
||||
for g in state['status'].status_list), 'Ordinary motion interrupted gizmo'
|
||||
assert done.result().result.error_code == 0
|
||||
wait(lambda: state['gizmo'].enabled)
|
||||
assert len(ids() - before) == 2
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
goal.pose.position.x -= .01
|
||||
finish(goal, *release(goal))
|
||||
print('PASS: ordinary action waits for completed gizmo action; next release works', flush=True)
|
||||
|
||||
# Pose/joints/file planners first request a lease, before planning from
|
||||
# the robot's current state. Exercise their actual client as well.
|
||||
goal = copy.deepcopy(state['gizmo'].actual)
|
||||
goal.pose.position.x -= .1
|
||||
epoch, before = release(goal)
|
||||
wait(lambda: len(ids() - before) == 1)
|
||||
low_id = next(iter(ids() - before))
|
||||
priority = MotionPriorityClient()
|
||||
completed = threading.Event()
|
||||
failures = []
|
||||
def acquire():
|
||||
try:
|
||||
priority.exchange('release-regression', True, 5.)
|
||||
except Exception as error:
|
||||
failures.append(error)
|
||||
finally:
|
||||
completed.set()
|
||||
started = time.monotonic()
|
||||
worker = threading.Thread(target=acquire, daemon=True)
|
||||
worker.start()
|
||||
try:
|
||||
wait(completed.is_set)
|
||||
assert not failures, failures
|
||||
elapsed = time.monotonic() - started
|
||||
assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
|
||||
for g in state['status'].status_list), 'Lease granted before gizmo completed'
|
||||
if values[4].double_value <= .03:
|
||||
assert elapsed > 2., 'Slow trajectory did not exercise the old 2 s RPC timeout'
|
||||
priority.exchange('release-regression', False, 5.)
|
||||
wait(lambda: state['gizmo'].enabled)
|
||||
print(f'PASS: planner lease waits for full gizmo action ({elapsed:.2f} s)', flush=True)
|
||||
finally:
|
||||
priority.close()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,112 @@
|
||||
"""Manual Webots speed regression: work -> 10 cm inward TCP translation.
|
||||
|
||||
Run against iiwa.launch.py simulate:=true. Checks actual feedback, both peak
|
||||
TCP speed and convergence, and prints elapsed simulation/wall time.
|
||||
"""
|
||||
import copy
|
||||
import math
|
||||
import time
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.parameter_client import AsyncParameterClient
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from controller_manager_msgs.srv import ListControllers
|
||||
from trajectory_msgs.msg import JointTrajectoryPoint
|
||||
from iiwa_msgs.msg import GizmoCommand, GizmoState
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = rclpy.create_node('gizmo_webots_speed')
|
||||
states = []
|
||||
node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', states.append, 10)
|
||||
publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
|
||||
|
||||
def wait(predicate, timeout=30.):
|
||||
deadline = time.monotonic() + timeout
|
||||
while not predicate():
|
||||
assert time.monotonic() < deadline, 'Timed out waiting for simulation'
|
||||
rclpy.spin_once(node, timeout_sec=.01)
|
||||
|
||||
def xyz(state):
|
||||
p = state.actual.pose.position
|
||||
return p.x, p.y, p.z
|
||||
|
||||
def stamp(state):
|
||||
t = state.actual.header.stamp
|
||||
return t.sec + t.nanosec * 1e-9
|
||||
|
||||
try:
|
||||
params = AsyncParameterClient(node, '/tcp_gizmo')
|
||||
assert params.wait_for_services(timeout_sec=30.)
|
||||
future = params.get_parameters([
|
||||
'simulation', 'use_sim_time', 'max_linear_speed', 'robot_description_semantic'])
|
||||
wait(future.done)
|
||||
values = future.result().values
|
||||
assert values[0].bool_value and values[1].bool_value, 'Webots simulation only'
|
||||
speed_limit = values[2].double_value
|
||||
work = ET.fromstring(values[3].string_value).find("group_state[@name='work']")
|
||||
assert work is not None
|
||||
controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
|
||||
assert controllers.wait_for_service(timeout_sec=30.)
|
||||
deadline = time.monotonic() + 30.
|
||||
while True:
|
||||
listing = controllers.call_async(ListControllers.Request())
|
||||
wait(listing.done)
|
||||
if any(c.name == 'iiwa_arm_controller' and c.state == 'active'
|
||||
for c in listing.result().controller):
|
||||
break
|
||||
assert time.monotonic() < deadline, 'Controller did not activate'
|
||||
rclpy.spin_once(node, timeout_sec=.1)
|
||||
wait(lambda: states and states[-1].enabled)
|
||||
action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
|
||||
assert action.wait_for_server(timeout_sec=5.)
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory.joint_names = [joint.attrib['name'] for joint in work]
|
||||
point = JointTrajectoryPoint(positions=[float(joint.attrib['value']) for joint in work])
|
||||
point.time_from_start.sec = 3
|
||||
goal.trajectory.points = [point]
|
||||
future = action.send_goal_async(goal)
|
||||
wait(future.done)
|
||||
assert future.result().accepted
|
||||
done = future.result().get_result_async()
|
||||
wait(done.done)
|
||||
assert done.result().result.error_code == 0
|
||||
wait(lambda: states[-1].enabled)
|
||||
initial = states[-1]
|
||||
target = copy.deepcopy(initial.actual)
|
||||
target.pose.position.x -= .1
|
||||
start = len(states) - 1
|
||||
epoch = initial.epoch
|
||||
wall_start = time.monotonic()
|
||||
publisher.publish(GizmoCommand(target=target, epoch=epoch))
|
||||
while states[-1].epoch == epoch:
|
||||
assert time.monotonic() - wall_start < 30., 'TCP did not reach 10 cm target'
|
||||
end = time.monotonic() + .025
|
||||
while time.monotonic() < end:
|
||||
rclpy.spin_once(node, timeout_sec=.005)
|
||||
assert states[-1].reason == 'target reached', states[-1].reason
|
||||
wait(lambda: initial.actual.pose.position.x - states[-1].actual.pose.position.x >= .099, 2.)
|
||||
samples = states[start:]
|
||||
speeds = [math.dist(xyz(a), xyz(b)) / (stamp(b) - stamp(a))
|
||||
for a, b in zip(samples, samples[1:]) if stamp(b) > stamp(a)]
|
||||
elapsed = stamp(samples[-1]) - stamp(initial)
|
||||
peak = max(speeds)
|
||||
distance = math.dist(xyz(initial), xyz(samples[-1]))
|
||||
print(f'limit={speed_limit:.3f} m/s; distance={distance:.6f} m; '
|
||||
f'simulation_time={elapsed:.3f} s; wall_time={time.monotonic() - wall_start:.3f} s; '
|
||||
f'mean={distance / elapsed:.4f} m/s; peak={peak:.4f} m/s', flush=True)
|
||||
assert peak <= speed_limit * 1.05 + .002, 'Measured TCP speed exceeds configured cap'
|
||||
# At 0.50 m/s, a 10 cm work-pose move must not consist of hundreds of
|
||||
# tiny acceleration-limited 64 ms steps. Keep margin for simulator load.
|
||||
if speed_limit >= .5:
|
||||
assert elapsed < 3., f'Artificial short-step speed bottleneck: {elapsed:.3f} s'
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,185 @@
|
||||
import threading
|
||||
import pytest
|
||||
from iiwa_planning.motion_priority import MotionLease, LeaseError, MotionSerialiser
|
||||
from iiwa_planning.motion_priority import wait_response
|
||||
|
||||
|
||||
def test_priority_acquisition_waits_for_full_gizmo_trajectory():
|
||||
from concurrent.futures import Future
|
||||
future = Future()
|
||||
timer = threading.Timer(2.1, lambda: future.set_result('granted'))
|
||||
timer.start()
|
||||
try:
|
||||
assert wait_response(future, timeout=None, available=lambda: True) == 'granted'
|
||||
finally:
|
||||
timer.join()
|
||||
|
||||
|
||||
def test_waiting_priority_fails_if_server_disappears():
|
||||
from concurrent.futures import Future
|
||||
future = Future()
|
||||
with pytest.raises(LeaseError, match='unavailable'):
|
||||
wait_response(future, timeout=None, available=lambda: False)
|
||||
assert future.cancelled()
|
||||
|
||||
|
||||
def test_overlapping_operations_have_independent_owners_and_release():
|
||||
owners = set()
|
||||
def exchange(owner, acquire, ttl):
|
||||
owners.add(owner) if acquire else owners.discard(owner)
|
||||
with MotionLease(exchange) as first:
|
||||
with MotionLease(exchange) as second:
|
||||
assert first.owner != second.owner
|
||||
assert len(owners) == 2
|
||||
assert owners == {first.owner}
|
||||
assert not owners
|
||||
|
||||
|
||||
def test_acquisition_fails_closed():
|
||||
def unavailable(*args):
|
||||
raise LeaseError('unavailable')
|
||||
with pytest.raises(LeaseError), MotionLease(unavailable):
|
||||
pytest.fail('motion entered without reservation')
|
||||
|
||||
|
||||
def test_renewal_failure_stops_operation():
|
||||
stopped = threading.Event()
|
||||
calls = []
|
||||
def exchange(owner, acquire, ttl):
|
||||
calls.append(acquire)
|
||||
if acquire and len(calls) > 1:
|
||||
raise LeaseError('lost')
|
||||
with pytest.raises(LeaseError):
|
||||
with MotionLease(exchange, on_lost=stopped.set, renew_interval=0.01) as lease:
|
||||
assert stopped.wait(1)
|
||||
lease.check()
|
||||
assert calls[-1] is False
|
||||
|
||||
|
||||
def test_release_does_not_mask_original_error():
|
||||
def exchange(owner, acquire, ttl):
|
||||
if not acquire:
|
||||
raise LeaseError('release failed')
|
||||
with pytest.raises(ValueError, match='original'):
|
||||
with MotionLease(exchange):
|
||||
raise ValueError('original')
|
||||
|
||||
|
||||
def test_stop_invalidates_plans_and_queued_requests():
|
||||
serial = MotionSerialiser()
|
||||
generation = serial.generation()
|
||||
serial.stop(lambda: None)
|
||||
with pytest.raises(LeaseError):
|
||||
serial.check(generation)
|
||||
serial.check(serial.generation())
|
||||
|
||||
|
||||
def test_motion_lock_serializes_workers():
|
||||
serial = MotionSerialiser()
|
||||
entered = threading.Event()
|
||||
finished = threading.Event()
|
||||
with serial.lock:
|
||||
def worker():
|
||||
entered.set()
|
||||
with serial.lock:
|
||||
finished.set()
|
||||
thread = threading.Thread(target=worker)
|
||||
thread.start()
|
||||
assert entered.wait(1)
|
||||
assert not finished.is_set()
|
||||
thread.join(1)
|
||||
assert finished.is_set()
|
||||
|
||||
|
||||
def _server_methods():
|
||||
"""Exercise real server orchestration without requiring MoveIt or ROS libraries."""
|
||||
import ast
|
||||
from pathlib import Path
|
||||
path = Path(__file__).parents[1] / 'scripts' / 'move_to_pose_server.py'
|
||||
source = ast.parse(path.read_text())
|
||||
cls = next(n for n in source.body if isinstance(n, ast.ClassDef))
|
||||
names = {'_plan_and_execute', '_check_operation', '_guarded', '_lease'}
|
||||
methods = [n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name in names]
|
||||
namespace = {'threading': threading, 'MotionLease': MotionLease,
|
||||
'PlanRequestParameters': object}
|
||||
exec(compile(ast.Module(body=methods, type_ignores=[]), str(path), 'exec'), namespace)
|
||||
return type('Server', (), {n: namespace[n] for n in names})
|
||||
|
||||
|
||||
def _fake_server(plan, execute):
|
||||
from types import SimpleNamespace
|
||||
server = _server_methods()()
|
||||
server._motion_serial = MotionSerialiser()
|
||||
server._operation = threading.local()
|
||||
server._operation.generation = server._motion_serial.generation()
|
||||
server._operation.lease = MotionLease()
|
||||
server._arm = SimpleNamespace(plan=plan)
|
||||
server._moveit = SimpleNamespace(execute=execute)
|
||||
server.get_logger = lambda: SimpleNamespace(error=lambda _: None)
|
||||
return server
|
||||
|
||||
|
||||
def test_stop_during_planning_prevents_execution():
|
||||
from types import SimpleNamespace
|
||||
executions = []
|
||||
def plan(**kwargs):
|
||||
server._motion_serial.stop(lambda: None)
|
||||
return SimpleNamespace(trajectory=object())
|
||||
server = _fake_server(plan, lambda *a, **kw: executions.append(a))
|
||||
with pytest.raises(LeaseError, match='stop'):
|
||||
server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
|
||||
assert not executions
|
||||
|
||||
|
||||
def test_failed_execution_is_reported_as_failure():
|
||||
from types import SimpleNamespace
|
||||
server = _fake_server(lambda **kw: SimpleNamespace(trajectory=object()),
|
||||
lambda *a, **kw: False)
|
||||
ok, _ = server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
|
||||
assert ok is False
|
||||
|
||||
|
||||
def test_queued_motion_reserves_before_waiting_for_planning_lock():
|
||||
server = _server_methods()()
|
||||
from types import SimpleNamespace
|
||||
reserved = threading.Event()
|
||||
completed = threading.Event()
|
||||
server._motion_serial = MotionSerialiser()
|
||||
server._operation = threading.local()
|
||||
server._gizmo_enabled = True
|
||||
server._stop_execution = lambda: None
|
||||
server._priority = SimpleNamespace(exchange=lambda owner, acquire, ttl:
|
||||
reserved.set() if acquire else None)
|
||||
with server._motion_serial.lock:
|
||||
thread = threading.Thread(target=lambda: server._guarded(completed.set))
|
||||
thread.start()
|
||||
assert reserved.wait(1)
|
||||
assert not completed.is_set()
|
||||
thread.join(1)
|
||||
assert completed.is_set()
|
||||
|
||||
|
||||
def test_disabled_lease_needs_no_ros_service():
|
||||
with MotionLease() as lease:
|
||||
lease.check()
|
||||
|
||||
|
||||
def test_failed_sequence_reports_nonzero_exit_and_releases_resources():
|
||||
import ast
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
path = Path(__file__).parents[1] / 'scripts' / 'motion_sequence_runner.py'
|
||||
tree = ast.parse(path.read_text())
|
||||
cls = next(n for n in tree.body if isinstance(n, ast.ClassDef))
|
||||
method = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == 'run')
|
||||
namespace = {'threading': threading, 'MotionLease': MotionLease, 'LeaseError': LeaseError}
|
||||
exec(compile(ast.Module(body=[method], type_ignores=[]), str(path), 'exec'), namespace)
|
||||
stopped, closed, done = threading.Event(), threading.Event(), threading.Event()
|
||||
def unavailable():
|
||||
raise LeaseError('priority unavailable')
|
||||
runner = SimpleNamespace(_discover_priority=unavailable, _priority=None,
|
||||
_abort_sequence=stopped.set, close_bag=closed.set,
|
||||
get_logger=lambda: SimpleNamespace(error=lambda _: None))
|
||||
namespace['run'](runner, done)
|
||||
assert stopped.is_set() and closed.is_set() and done.is_set()
|
||||
assert runner.exit_code != 0
|
||||
@@ -0,0 +1,81 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <iiwa_planning/tcp_gizmo_policy.hpp>
|
||||
using iiwa_planning::GizmoPolicy;
|
||||
TEST(GizmoPolicy, LeasesAreBarriersAndExpiredCommandsNeverReplay) {
|
||||
GizmoPolicy gate;
|
||||
EXPECT_TRUE(gate.accept(0, 1., .3));
|
||||
EXPECT_TRUE(gate.acquire("planning", 2., 1.));
|
||||
EXPECT_EQ(gate.epoch(), 1u);
|
||||
EXPECT_FALSE(gate.allowed(1.5, 0., true));
|
||||
EXPECT_FALSE(gate.hasTarget());
|
||||
EXPECT_TRUE(gate.allowed(3., 0., true));
|
||||
EXPECT_FALSE(gate.accept(0, 3., .3));
|
||||
EXPECT_TRUE(gate.accept(1, 3., .3));
|
||||
EXPECT_TRUE(gate.expired(3.31));
|
||||
}
|
||||
TEST(GizmoPolicy, RenewalAndMultipleOwnersHoldThroughPauses) {
|
||||
GizmoPolicy gate;
|
||||
gate.acquire("a", 2., 0.); gate.acquire("b", 5., 0.);
|
||||
gate.acquire("a", 4., 1.);
|
||||
EXPECT_EQ(gate.epoch(), 2u);
|
||||
gate.release("b");
|
||||
EXPECT_FALSE(gate.allowed(4., 100., true));
|
||||
EXPECT_TRUE(gate.allowed(5.1, 100., true));
|
||||
}
|
||||
TEST(GizmoPolicy, NormalTrajectoryWaitsForSimulationDeadlineAndSettling) {
|
||||
GizmoPolicy gate;
|
||||
gate.normal(12.);
|
||||
EXPECT_FALSE(gate.allowed(100., 11., true));
|
||||
EXPECT_FALSE(gate.allowed(100., 12.1, false));
|
||||
EXPECT_TRUE(gate.allowed(100., 12.1, true));
|
||||
EXPECT_FALSE(gate.hasTarget());
|
||||
}
|
||||
TEST(GizmoPolicy, RejectsInvalidLeaseAndBlocksActions) {
|
||||
GizmoPolicy gate;
|
||||
EXPECT_FALSE(gate.acquire("", 1., 0.));
|
||||
EXPECT_FALSE(gate.acquire("x", -1., 0.));
|
||||
gate.action(true);
|
||||
EXPECT_FALSE(gate.allowed(0., 0., true));
|
||||
gate.action(false);
|
||||
EXPECT_FALSE(gate.allowed(0., 0., false));
|
||||
EXPECT_TRUE(gate.allowed(0., 0., true));
|
||||
}
|
||||
|
||||
TEST(GizmoPolicy, LeaseReleaseRequiresRobotToSettle) {
|
||||
GizmoPolicy gate;
|
||||
gate.acquire("planning", 2., 0.);
|
||||
gate.release("planning");
|
||||
EXPECT_FALSE(gate.allowed(1., 1., false));
|
||||
EXPECT_TRUE(gate.allowed(1., 1., true));
|
||||
}
|
||||
|
||||
TEST(LowGoalBarrier, DelayedAcceptanceCannotPassHighBarrier) {
|
||||
iiwa_planning::LowGoalBarrier barrier;
|
||||
EXPECT_TRUE(barrier.begin());
|
||||
EXPECT_FALSE(barrier.idle());
|
||||
barrier.accepted(true);
|
||||
EXPECT_FALSE(barrier.idle());
|
||||
// A high-priority request must drain the already accepted low step.
|
||||
EXPECT_FALSE(barrier.begin());
|
||||
barrier.terminal();
|
||||
EXPECT_TRUE(barrier.idle());
|
||||
}
|
||||
TEST(LowGoalBarrier, RejectedGoalReleasesBarrierWithoutCancellation) {
|
||||
iiwa_planning::LowGoalBarrier barrier;
|
||||
EXPECT_TRUE(barrier.begin());
|
||||
EXPECT_FALSE(barrier.begin());
|
||||
barrier.accepted(false);
|
||||
EXPECT_TRUE(barrier.idle());
|
||||
EXPECT_TRUE(barrier.begin());
|
||||
barrier.accepted(true);
|
||||
EXPECT_FALSE(barrier.idle());
|
||||
barrier.terminal();
|
||||
EXPECT_TRUE(barrier.idle());
|
||||
}
|
||||
|
||||
TEST(GizmoPolicy, ReachedTargetDoesNotRequestServoMotion) {
|
||||
EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(0., 0.));
|
||||
EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(1e-6, 1e-5));
|
||||
EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(1e-3, 0.));
|
||||
EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(0., .01));
|
||||
}
|
||||
Reference in New Issue
Block a user