[ROS2] rclpy action client freeze

edit

Hello,

I have a two nodes: action server and action client. The action is custom and it's purpose is to control Universal Robot (ur_modern_driver from github do have so limits). Both nodes follow designs from gihub, tutorials etc. However, wired and undeterministic bug appears in my code. The client node freezes on either "waiting for goal accepted" or "waiting for result". Bellow is an error after keyboard interrupt:

[environment-3] The following exception was never retrieved: Failed to take feedback with an action client: error not set
[environment-3] Traceback (most recent call last):
[environment-3] File "/home/pippin/ros2_scm_ws/install/rl_assemble/lib/rl_assemble/environment", line 11, in <module>
[environment-3]
load_entry_point('rl-assemble', 'console_scripts', 'environment')()
[environment-3] File "/home/pippin/ros2_scm_ws/build/rl_assemble/rl_assemble/main.py", line 13, in main
[environment-3] play.learn()
[environment-3] File "/home/pippin/ros2_scm_ws/build/rl_assemble/rl_assemble/learning/common/play.py", line 76, in learn
[environment-3] score, _, _ = self._run_env(True)
[environment-3] File "/home/pippin/ros2_scm_ws/build/rl_assemble/rl_assemble/learning/common/play.py", line 105, in _run_env
[environment-3] next_state, reward, done, info = self._env.step(scaled_action)
[environment-3] File "/home/pippin/ros2_scm_ws/build/rl_assemble/rl_assemble/env/assemble_env.py", line 106, in step
[environment-3] while not self._robot.force_move(pose, zero_sensor=zero_sensor):
[environment-3] File "/home/pippin/ros2_scm_ws/build/rl_assemble/rl_assemble/env/robot.py", line 132, in force_move
[environment-3]
self._executor.spin_once()
[environment-3] File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/executors.py", line 695, in spin_once
[environment-3] handler, entity, node = self.wait_for_ready_callbacks(timeout_sec=timeout_sec) [environment-3] File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/executors.py", line 649, in wait_for_ready_callbacks [environment-3] return next(self._cb_iter)
[environment-3] File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/executors.py", line 506, in _wait_for_ready_callbacks [environment-3]
service_capsules.append(context_stack.enter_context(srv.handle))

And here is sample of code:

def force_move(self,
               point: Pose,
               pid_params: Tuple[float, ...] = (2.0, 0.2, 0.0, 1.0, 0.0, 0.0),
               ft_select: Tuple[bool, ...] = (False, False, True, False, False, False),
               ft_target: Tuple[float, ...] = (0.0, 0.0, -1.5, 0.0, 0.0, 0.0),
               zero_sensor: bool = False,
               velocity: float = 0.01,
               acceleration: float = 0.01) -> bool:
    self._force_move_action.wait_for_server()
    goal_msg = self._construct_force_move_goal(pid_params, ft_select, ft_target)
    goal_msg.zero_sensor = zero_sensor
    goal_msg.trajectory.points.append(self._construct_point_msg(point, velocity, acceleration))

    self._action_done_event.clear()
    self._action_status_flag.clear()

    send_goal_future = self._force_move_action.send_goal_async(goal_msg)
    send_goal_future.add_done_callback(self._goal_response_callback)

    while not self._action_done_event.is_set():
        self._executor.spin_once()

    return self._action_status_flag.is_set()

def _goal_response_callback(self, future):
    goal_handle = future.result()
    if not goal_handle.accepted:
        self._node.get_logger().info('Goal rejected!')
        self._action_done_event.set()
        return
    get_result_future = goal_handle.get_result_async()
    get_result_future.add_done_callback(self._get_result_callback)

def _get_result_callback(self, future):
    status = future.result().status
    if status == GoalStatus.STATUS_SUCCEEDED:
        self._action_status_flag.set()
    self._action_done_event.set()

System setup:

• Ubuntu 18.04 ROS2
• Dashing Diamond via deb

I would be grateful for any help.

Greg.