How to ensure a service (start_rapid/stop_rapid) is done processing

[dave992]

For a project, I am trying to automate several motions using the abb_robot_driver with our IRB1200. To start the EGM session I automated the example ex2_rws_and_egm_6axis_robot, in particular, the following service calls:

rosservice call /rws/stop_rapid "{}"
rosservice call /rws/pp_to_main "{}"
rosservice call /rws/start_rapid "{}"
rosservice call /rws/sm_addin/start_egm_joint "{}"

From the terminal, the abovementioned commands work fine. However, when called in quick succession via a python script, it seems the stop_rapid and start_rapid service calls are not fully executed yet on the controller side when I try to execute the next service call. This then results in an error as the follow-up service calls expect RAPID to be started/stopped.

To illustrate this, here is an example of when a program is still running:

1. Prerequisite: A RAPID program is still active
2. Call stop_rapid service, it returns successfully (status code 1 and empty message)
3. Call pp_to_main service, it fails with status code 3004 and message RAPID has not been stopped

[DEBUG] [1648129932.051213]: connecting to IRB1200-Linux-PC 44455
[DEBUG] [1648129932.057497]: Service [stop_rapid] result status code: 1, message: 
[DEBUG] [1648129932.058719]: connecting to IRB1200-Linux-PC 44455
[DEBUG] [1648129932.059738]: Service [pp_to_main] result status code: 3004, message: "RAPID has not been stopped"
samxl_abb_utilities.core.ABBServiceFailed: Service [pp_to_main] failed with status code: 3004, message: "RAPID has not been stopped"

The stop_rapid service was completed successfully, but the pp_to_main call then still fails as the previous call has not yet finished processing (I assume). Sometimes the call does succeed, but for me most of the time it does not.

Should I expect all service requests to be handled asynchronously? If so, is there a way to check when it is done processing?

[gavanderhoorn]

It's been a while since I've worked with this specific service, but as a general comment: all RWS services manipulate either the StateMachine program, or another part of the runtime system state of the IRC5 controller.

Some services can take longer than others to affect something. So to close the loop (in this case), you'd subscribe to the system_states topic (carrying abb_robot_msgs/msg/SystemState messages), and inspect that state using the fields in that message. Other topics carry the state of other parts of the system (such as whether EGM is active or not, what its state is (converging or not), or in which state the StateMachine currently is).

I can't point you to a message/field for "pp-to-main" specifically right now, but in general I don't believe you can only rely on service results to coordinate / time your sequence.

(this is true of ROS services in general btw: all services which do not experience a failure to send over the request and invoke the callback in the server will return true, regardless of whether the action initiated by that invocation was successful. Communicating success or failure of that would/should be done using fields in the service response, such as the result_code field we have in abb_robot_msgs/srv/TriggerWithResultCode)

So for motors_on fi, you'd:

1. look at systems_states.motors_on, and if it's false, then
2. call the set_motors_on service, then
3. check the service response to see it was successful (ie: check the ROS svc client returns true), and
4. check the result_code (it should be RC_SUCCESS (1)). Finally
5. keep an eye on systems_states.motors_on and wait till it becomes true

at this point you'd know the service was successful, and the system state was updated as requested.

---

Edit: one possible design here would essentially be a client-side statemachine which models controller state (based on information carried by the various topics) and transitions through intermediate states (which trigger the respective services) to end up in the desired state (ie: the-motors-are-on or egm-joint-mode-is-active-and-ready).

---

Edit 2: for start_rapid you could look at system_states.rapid_running.

[dave992]

That clears things up, thanks! I will change my approach to instead monitor the topics to see if and when the state changes after a successful service call (RC_SUCCES = 1).

[gavanderhoorn]

It would have been an option to include checking for motors_on (etc) in the server service callback of course, but that's not how they were implemented.