[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/ros2scmws/build/rlassemble/rlassemble/main.py", line 13, in main
[environment-3] play.learn()
[environment-3] File "/home/pippin/ros2scmws/build/rlassemble/rlassemble/learning/common/play.py", line 76, in learn
[environment-3] score, _, _ = self.runenv(True)
[environment-3] File "/home/pippin/ros2scmws/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/ros2scmws/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 spinonce
[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:
- Ubuntu 18.04 ROS2
- Dashing Diamond via deb
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