Robotics StackExchange | Archived questions

[ROS2] rclpy action client freeze

Hello,

I have a two nodes: action server and action client. The action is custom and it's purpose is to control Universal Robot (urmoderndriver 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/ros2scmws/install/rlassemble/lib/rlassemble/environment", line 11, in
[environment-3]
loadentrypoint('rl-assemble', 'consolescripts', 'environment')()
[environment-3] File "/home/pippin/ros2
scmws/build/rlassemble/rlassemble/main.py", line 13, in main
[environment-3] play.learn()
[environment-3] File "/home/pippin/ros2
scmws/build/rlassemble/rlassemble/learning/common/play.py", line 76, in learn
[environment-3] score, _, _ = self.
runenv(True)
[environment-3] File "/home/pippin/ros2
scmws/build/rlassemble/rlassemble/learning/common/play.py", line 105, in _runenv
[environment-3] nextstate, reward, done, info = self.env.step(scaledaction)
[environment-3] File "/home/pippin/ros2
scmws/build/rlassemble/rlassemble/env/assembleenv.py", line 106, in step
[environment-3] while not self.robot.forcemove(pose, zerosensor=zerosensor):
[environment-3] File "/home/pippin/ros2scmws/build/rlassemble/rlassemble/env/robot.py", line 132, in forcemove
[environment-3]
self.
executor.spinonce()
[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.waitforreadycallbacks(timeoutsec=timeoutsec) [environment-3] File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/executors.py", line 649, in waitforreadycallbacks [environment-3] return next(self.cbiter)
[environment-3] File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/executors.py", line 506, in waitforreadycallbacks [environment-3]
servicecapsules.append(contextstack.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:

I would be grateful for any help.

Greg.

Asked by souphis on 2019-07-31 01:19:12 UTC

Comments

I have the same problem. Even with a timeout, it just freezes forever.
rclpy.spin_until_future_complete(self, future, None, 1.0)

Asked by madmax on 2019-12-31 06:02:49 UTC

Answers