search

jsk-ros-pkg / jsk_pr2eus

PR2 euslisp packages
https://github.com/jsk-ros-pkg/jsk_pr2eus
4 stars 41 forks source link

Add roseus_resume support #488

Closed Affonso-Gui closed 2 years ago

Affonso-Gui commented 2 years ago

Solution for https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/485.

if we need to install all action client, may be we try to find them from program code

I have taken this approach, searching and stopping all instances of ros::simple-action-client registered as slots in the robot-interface instance. https://github.com/Affonso-Gui/roseus_resume/blob/eus10/euslisp/interruption-handler.l#L60-L70

The initialization code is then moved to robot-interface, and can be enabled by default in other robots without any modification. Roseus_resume will be automatically disabled on eus9 or when the package is not found, and can also be manually disabled by setting the *enable-roseus-resume* variable to nil.

(setq *enable-roseus-resume* nil)
(pr2-init)

Other (user-defined) actions can also be registered as interruption targets by further calling the roseus_resume:install-interruption-handler function.

(pr2-init)
(roseus_resume:install-interruption-handler *ri*
  my-action-client
  my-other-action-client)

If the user doesn't want to interrupt a certain action, this can be achieved by overwriting the :get-action-status, :interrupt-angle-vector, or roseus_resume:on-interruption. The last one is preferred for local overwrites.

(handler-bind ((roseus_resume:on-interruption
                #'(lambda (c)
                    (send *ri* :interrupt-angle-vector
                          (remove gripper-controller (send c :status))))))
  (do-something))
knorth55 commented 2 years ago

Good!