Skip to content

Commit a141e4b

Browse files
theodomtheodom
committed
added taskstate
1 parent ba0438b commit a141e4b

2 files changed

Lines changed: 100 additions & 1 deletion

File tree

‎debug_log/gek_4_log.txt‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
theo@theo-mbp:~/MP_percussionModule$ ros2 launch percussion_task_manager task_system.launch.py
2+
[INFO] [launch]: All log files can be found below /home/theo/.ros/log/2026-04-13-14-18-49-222348-theo-mbp-216244
3+
[INFO] [launch]: Default logging verbosity is set to INFO
4+
[INFO] [task_manager_node-1]: process started with pid [216247]
5+
[INFO] [capture_service_node-2]: process started with pid [216248]
6+
[INFO] [percussion_motion_node-3]: process started with pid [216249]
7+
[INFO] [static_transform_publisher-4]: process started with pid [216250]
8+
[INFO] [static_transform_publisher-5]: process started with pid [216251]
9+
[static_transform_publisher-4] [WARN] [1776082729.351168962] []: Old-style arguments are deprecated; see --help for new-style arguments
10+
[static_transform_publisher-5] [WARN] [1776082729.364113050] []: Old-style arguments are deprecated; see --help for new-style arguments
11+
[static_transform_publisher-4] [INFO] [1776082729.373503425] [tool0_to_camera_broadcaster]: Spinning until stopped - publishing transform
12+
[static_transform_publisher-4] translation: ('0.000000', '-0.127750', '0.044510')
13+
[static_transform_publisher-4] rotation: ('0.000000', '0.000000', '1.000000', '0.000796')
14+
[static_transform_publisher-4] from 'tool0' to 'camera_frame'
15+
[static_transform_publisher-5] [INFO] [1776082729.397191784] [world_to_base_link_broadcaster]: Spinning until stopped - publishing transform
16+
[static_transform_publisher-5] translation: ('0.000000', '0.000000', '0.000000')
17+
[static_transform_publisher-5] rotation: ('0.382683', '0.000000', '0.000000', '0.923880')
18+
[static_transform_publisher-5] from 'world' to 'base_link'
19+
[capture_service_node-2] [INFO] [1776082730.131493528] [capture_service_node]: Capture service node ready.
20+
[task_manager_node-1] [INFO] [1776082730.169589571] [task_manager]: State -> IDLE
21+
[task_manager_node-1] [INFO] [1776082730.170123248] [task_manager]: Task manager node started.
22+
[percussion_motion_node-3] [INFO] [1776082730.185615986] [percussion_motion]: Connecting to robot at 169.254.0.22 …
23+
[percussion_motion_node-3] [INFO] [1776082731.435700080] [percussion_motion]: RTDE connection established.
24+
[percussion_motion_node-3] [INFO] [1776082731.436518808] [percussion_motion]: Percussion motion node ready.
25+
[capture_service_node-2] [INFO] [1776082733.799123416] [capture_service_node]: Received capture request: timeout=0
26+
[capture_service_node-2] [INFO] [1776082734.923426319] [capture_service_node]: marker: [ 0.05052327 -0.03257366 0.81069843 2.1190085 -1.55334818 0.18555894]
27+
[percussion_motion_node-3] [INFO] [1776082735.026028265] [percussion_motion]: Accepting goal: MOVE_TO_MARKER
28+
[percussion_motion_node-3] [INFO] [1776082735.027409080] [percussion_motion]: Executing motion: MOVE_TO_MARKER | target=(-0.0505, 0.1603, 0.7662) | pose_offset=(-0.070, 0.000, 0.000)
29+
[percussion_motion_node-3] [INFO] [1776082735.034892191] [percussion_motion]: [MOVE_TO_MARKER] phase: APPROACHING
30+
[percussion_motion_node-3] [INFO] [1776082735.035554180] [percussion_motion]: Extra: Goal pose: [0.6476854894683122, 0.4278166778123527, 0.4973828792403554, -2.820067099786316, -0.9080005670663817, -0.8511557198054827]
31+
[percussion_motion_node-3] [INFO] [1776082740.639911858] [percussion_motion]: [MOVE_TO_MARKER] phase: MOVING_TO_MARKER
32+
[percussion_motion_node-3] RTDEControlInterface: RTDE control script is not running!
33+
[percussion_motion_node-3] [INFO] [1776082740.642741009] [percussion_motion]: [MOVE_TO_MARKER] phase: MARKER_REACHED
34+
[percussion_motion_node-3] [INFO] [1776082740.660900837] [percussion_motion]: Accepting goal: MOVE_TO_CONTACT
35+
[percussion_motion_node-3] [INFO] [1776082740.664967892] [percussion_motion]: Executing motion: MOVE_TO_CONTACT | target=(0.0000, 0.0000, 0.0000) | pose_offset=(0.010, 0.000, 0.000)
36+
[percussion_motion_node-3] RTDEControlInterface: RTDE control script is not running!
37+
[percussion_motion_node-3] [ERROR] [1776082740.667763034] [percussion_motion]: Unhandled exception in motion execution: poseTrans() function did not succeed!
38+
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
39+
[static_transform_publisher-5] [INFO] [1776082748.015084667] [rclcpp]: signal_handler(SIGINT/SIGTERM)
40+
[static_transform_publisher-4] [INFO] [1776082748.015399885] [rclcpp]: signal_handler(SIGINT/SIGTERM)
41+
[percussion_motion_node-3] [INFO] [1776082748.017729254] [percussion_motion]: Shutting down — stopping robot script.
42+
[percussion_motion_node-3] Failed to publish log message to rosout: publisher's context is invalid, at ./src/rcl/publisher.c:423
43+
[percussion_motion_node-3] RTDEControlInterface: RTDE control script is not running!
44+
[capture_service_node-2] entering camera init
45+
[capture_service_node-2] exiting camera init
46+
[task_manager_node-1] Traceback (most recent call last):
47+
[task_manager_node-1] File "/home/theo/MP_percussionModule/install/percussion_task_manager/lib/percussion_task_manager/task_manager_node", line 33, in <module>
48+
[task_manager_node-1] sys.exit(load_entry_point('percussion-task-manager==0.0.1', 'console_scripts', 'task_manager_node')())
49+
[task_manager_node-1] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
50+
[task_manager_node-1] File "/home/theo/MP_percussionModule/install/percussion_task_manager/lib/python3.12/site-packages/percussion_task_manager/task_manager_node.py", line 294, in main
51+
[task_manager_node-1] rclpy.shutdown()
52+
[task_manager_node-1] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown
53+
[task_manager_node-1] _shutdown(context=context)
54+
[task_manager_node-1] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown
55+
[task_manager_node-1] context.shutdown()
56+
[task_manager_node-1] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown
57+
[task_manager_node-1] self.__context.shutdown()
58+
[task_manager_node-1] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333
59+
[percussion_motion_node-3] Traceback (most recent call last):
60+
[percussion_motion_node-3] File "/home/theo/MP_percussionModule/install/percussion_motion/lib/percussion_motion/percussion_motion_node", line 33, in <module>
61+
[percussion_motion_node-3] sys.exit(load_entry_point('percussion-motion==0.0.0', 'console_scripts', 'percussion_motion_node')())
62+
[percussion_motion_node-3] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
63+
[percussion_motion_node-3] File "/home/theo/MP_percussionModule/install/percussion_motion/lib/python3.12/site-packages/percussion_motion/percussion_motion_node.py", line 333, in main
64+
[percussion_motion_node-3] rclpy.shutdown()
65+
[percussion_motion_node-3] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown
66+
[percussion_motion_node-3] _shutdown(context=context)
67+
[percussion_motion_node-3] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown
68+
[percussion_motion_node-3] context.shutdown()
69+
[percussion_motion_node-3] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown
70+
[percussion_motion_node-3] self.__context.shutdown()
71+
[percussion_motion_node-3] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333
72+
[capture_service_node-2] Traceback (most recent call last):
73+
[capture_service_node-2] File "/home/theo/MP_percussionModule/install/percussion_perception/lib/percussion_perception/capture_service_node", line 33, in <module>
74+
[capture_service_node-2] sys.exit(load_entry_point('percussion-perception==0.0.1', 'console_scripts', 'capture_service_node')())
75+
[capture_service_node-2] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
76+
[capture_service_node-2] File "/home/theo/MP_percussionModule/install/percussion_perception/lib/python3.12/site-packages/percussion_perception/capture_service_node.py", line 104, in main
77+
[capture_service_node-2] rclpy.shutdown()
78+
[capture_service_node-2] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown
79+
[capture_service_node-2] _shutdown(context=context)
80+
[capture_service_node-2] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown
81+
[capture_service_node-2] context.shutdown()
82+
[capture_service_node-2] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown
83+
[capture_service_node-2] self.__context.shutdown()
84+
[capture_service_node-2] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333
85+
[INFO] [static_transform_publisher-4]: process has finished cleanly [pid 216250]
86+
[INFO] [static_transform_publisher-5]: process has finished cleanly [pid 216251]
87+
[ERROR] [task_manager_node-1]: process has died [pid 216247, exit code 1, cmd '/home/theo/MP_percussionModule/install/percussion_task_manager/lib/percussion_task_manager/task_manager_node --ros-args -r __node:=task_manager'].
88+
[ERROR] [capture_service_node-2]: process has died [pid 216248, exit code 1, cmd '/home/theo/MP_percussionModule/install/percussion_perception/lib/percussion_perception/capture_service_node --ros-args -r __node:=capture_service_node --params-file /tmp/launch_params_8vpkjt2r'].
89+
[ERROR] [percussion_motion_node-3]: process has died [pid 216249, exit code 1, cmd '/home/theo/MP_percussionModule/install/percussion_motion/lib/percussion_motion/percussion_motion_node --ros-args

‎src/percussion_task_manager/percussion_task_manager/task_manager_node.py‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
class TaskState(str, Enum):
1616
IDLE = 'IDLE'
17+
TASK_REQUESTED = 'TASK_REQUESTED'
1718
CAPTURING = 'CAPTURING'
1819
POSE_ACQUIRED = 'POSE_ACQUIRED'
1920
MOVING_TO_WEDGELOCK = 'MOVING_TO_WEDGELOCK'
@@ -84,7 +85,7 @@ def _build_sequence(self, marker_pose: Pose6D) -> List[dict]:
8485
{
8586
'motion_type': 'RELATIVE_MOVE', # Rotate Tool to face wedgelock
8687
'marker_pose': _make_pose6d(),
87-
'approach_offset': [0.0, 0.0, 0.0, 0.0, -1.4600, 0.0], # TCP frame
88+
'approach_offset': [0.0, 0.0, 0.0, 0.0, -1.5701, 0.0], # TCP frame
8889
},
8990
{
9091
'motion_type': 'RELATIVE_MOVE', # Move towards wedgelock (sideways) for contact 2D
@@ -129,6 +130,11 @@ def _build_return_sequence(self) -> List[dict]:
129130
'marker_pose': _make_pose6d(),
130131
'approach_offset': [0.0, 0.05, -0.10, 0.0, 0.0, 0.0], # TCP frame
131132
},
133+
{
134+
'motion_type': 'RELATIVE_MOVE', # Rotate Tool to unface wedgelock
135+
'marker_pose': _make_pose6d(),
136+
'approach_offset': [0.0, 0.0, 0.0, 0.0, 1.5701, 0.0], # TCP frame
137+
},
132138
{
133139
'motion_type': 'RETURN_HOME',
134140
'marker_pose': _make_pose6d(),
@@ -211,6 +217,10 @@ def _on_capture_done(self, future) -> None:
211217

212218

213219
self.publish_state(TaskState.POSE_ACQUIRED)
220+
221+
# Rework to be more general motion.
222+
223+
214224
self._sequence = self._build_sequence(selected.pose)
215225
self._returning = False
216226
self._execute_next_step()

0 commit comments

Comments
 (0)