Refactored fault-awareness and stop action

[AstroStucky]

Summary of Changes

• Restructured the arm interface to enable stopping a trajectory.
• Stop [arm] action and grind action added as demonstrations of the refactor.
• The underline logic for stow and unstow, being very similar, has been consolidated into its own mixin class.
• Added the ability to switch controllers, which is required by the grind action.
• The arm now must be "owned" or "checked out" by whatever facility wishes to use it through the new ArmInterface class. This enables a stop to work even during the trajectory planning phase (though you will have to wait for planning to complete) and allows the interface to reject new requests from other arm actions if the arm is already in use. Previously if a second arm action was called during the execution of the first, they would both attempt to send conflicting goals.
• Fixed some broken lines of code left over in trajectory_planner from the previous PR. A lot of repetitive changes here, feel free to skim this in your review.
• Reorganized the contents of the /src/ow_lander/actions directory. Now all action servers are defined inside in either arm.py or lander.py. This was mostly done to streamline import statements of scripts in the scripts directory. Now those statements will not have to be modified whenever an action is added or removed.

Tests

All of the following tests can be done in order and in the same sim instance with 3 different terminals open. For setup:

1. Source devel/setup.bash in 3 different terminals.
2. In terminal 1 Start the sim roslaunch ow atacama_y1a.launch

Help Messages

Verify help messages make sense. Call:

1. rosrun ow_lander stop_client.py --help
2. rosrun ow_lander grind_client.py --help
   In any free terminal.

Test grind

1. In terminal 2 Call the grind action rosrun ow_lander grind_client.py
   Allow the action to complete. Verify the trajectory looks the same as in noetic-devel

Test stop and arm ownership

1. Send arm back to the stow position
2. In terminal 2 Call the stop action rosrun ow_lander stop_client.py
   You should see the following in the sim output

[INFO] [1670957205.843679, 1916524812.767500]: Stop action started
[ERROR] [1670957205.844469, 1916524812.767500]: Stop: Aborted - No arm trajectory to stop
[INFO] [1670957205.845229, 1916524812.767500]: Stop action complete

3. In terminal 2 Call an unstow rosrun ow_lander unstow_client.py
4. While the unstow executes, in terminal 3 call stop rosrun ow_lander stop_client.py
   The scoop will stop moving and you should see

[INFO] [1670957352.817509, 1916524959.652500]: Unstow action started
[INFO] [1670957356.524874, 1916524963.355000]: Stop action started
[INFO] [1670957356.525842, 1916524963.357500]: Stop: Succeded - Arm trajectory stopped
[INFO] [1670957356.526580, 1916524963.357500]: Stop action complete
[ERROR] [1670957356.530696, 1916524963.362500]: Unstow: Aborted - Stop was called; trajectory execution ceased
[INFO] [1670957356.531844, 1916524963.362500]: Unstow action complete

5. In terminal 2 call unstow and allow it to complete this time rosrun ow_lander unstow_client.py
6. In terminal 2 call stow rosrun ow_lander stow_client.py
7. While the stow executes, in terminal 3 attempt to call an unstow rosrun ow_lander unstow_client.py
   Allow stow to complete, and then you should see the following in the sim output

[INFO] [1670957614.120125, 1916525220.767500]: Stow action started
[INFO] [1670957620.199082, 1916525226.842500]: Unstow action started
[ERROR] [1670957620.200161, 1916525226.842500]: Unstow: Aborted - Arm is already checked out by the Stow action server
[INFO] [1670957620.201090, 1916525226.845000]: Unstow action complete
[INFO] [1670957628.880321, 1916525235.505000]: Stow: Succeded - Arm trajectory succeeded
[INFO] [1670957628.881281, 1916525235.505000]: Stow action complete

Stop due to arm fault and continue flag

1. Send the arm back to the stow position.
2. In terminal 2 call an unstow
3. While that unstow executes, in rqt_gui go to Dynamic Reconfigure > Faults and tick any of the *_joint_lock_failure arm faults.
   The arm should immediately stop and you'll see the following in sim output

[INFO] [1670957968.734658, 1916524853.582500]: Unstow action started
[ INFO] [1670957972.629881697, 1916524857.477499962]: j_shou_pitch joint locked!
[ERROR] [1670957972.672549, 1916524857.520000]: Unstow: Aborted - Stop was called; trajectory execution ceased
[INFO] [1670957972.673854, 1916524857.520000]: Unstow action complete

4. In rqt_gui, untick all arm faults and tick the arm_motion_continues_in_fault flag (so that its turned on).
5. Now in terminal 2 call unstow and allow it to complete
6. In terminal 2, call stow
7. While stow executes, in rqt_gui tick one of the *_joint_lock_failure arm faults. The arm should continue, but move off its intended trajectory, it may never even reach the stow position depending on which joint you locked. It will complete its trajectory, but may not be in the correct position at the end of it. You should expect the following in the sim output only with the name of the joint you chose to lock.

[INFO] [1670958209.410521, 1916525094.070000]: Stow action started
[ INFO] [1670958215.114210598, 1916525099.772500038]: j_prox_pitch joint locked!
[INFO] [1670958223.459267, 1916525108.112500]: Stow: Succeded - Arm trajectory succeeded
[INFO] [1670958223.461082, 1916525108.112500]: Stow action complete

Freestyle

There are more scenarios for this to test than I can list here. Feel free to spam it with multiple action requests and see if you can break it. Share your findings.

[AstroStucky]

I saw a need for a last minute fix, which was just pushed. If you have already tested, I doubt this change is big enough to justify a retest.

[kmdalal]

Probably unrelated to this PR, but I'm getting the following message every time I run an action client script: the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'. Aside from this the scripts have worked fine so far.

[kmdalal]

Out of this PR's scope, but the behavior of the arm after it finishes an operation with a faulted joint(s) with continue arm in fault selected, and then clearing the faults, is not always intuitive and sometimes odd. This issue pre-existed and there is an Epic/Jira for it.

[kmdalal]

The tests pass, and I tried a few more. Regarding the code, my main comment is on the organization and file naming, which I don't find intuitive.

• the actions directory seems too insignificant and I think its contents would be fine in the parent directory, since that is all action support. It's also a bit confusing that there is an action directory at another level.
• the files arm.py and lander.py could have more descriptive names (especially since the arm is part of the lander) and perhaps be combined into one file? Or, if you want to keep arm action servers together, the others could be each in their own file with more specific names.

[AstroStucky]

Probably unrelated to this PR, but I'm getting the following message every time I run an action client script: the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'. Aside from this the scripts have worked fine so far.

I think this is just ROS letting you know you're overdue for a package update.

[AstroStucky]

In regards to the actions directory. I am amenable to removing it in favor of having a flat directory hierarchy. I would strongly prefer to keep at least the arm action servers all in the same file because it means we don't have to modify the import statement in arm_action_servers.py each time a new action is added/removed.

Even though it may result in a longish file, I think the simplest solution is to put all actions into a single file called actions.py that would exist in the ow_lander/src/ow_lander directory. The import statements benefit from this by staying idiomatic and brief.
e.g.

from ow_lander import actions
light_server = actions.LightSetInensityServer()
unstow_server = actions.UnstowServer()

(not a real application, just a quick example)

[anthonykoutroulis]

Probably unrelated to this PR, but I'm getting the following message every time I run an action client script: the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'. Aside from this the scripts have worked fine so far.

I get this too when running the scripts, but not with their previous iterations. Not sure what causes this or if it's indicative of any issue though, and strangely I don't have the rosdep package on my system.

[kmdalal]

Probably unrelated to this PR, but I'm getting the following message every time I run an action client script: the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'. Aside from this the scripts have worked fine so far.

I get this too when running the scripts, but not with their previous iterations. Not sure what causes this or if it's indicative of any issue though, and strangely I don't have the rosdep package on my system.

Following advice from a ROS forum, I just did a 'rosdep update' (not sudo), a bunch of packages got updated, and this message went away.

@anthonykoutroulis , rosdep is not a package but an executable that should be in your search path (mine's in /usr/bin). If you don't have it, ask SG to install it for you.

[AstroStucky]

It's interesting to me you don't see the rosdep notice occur in noetic-devel. I did remove some ros package import statements that looked unnecessary to me during the refactor, maybe that is related. If performing a rosdep update makes the message go away, I would assume it's just ROS performing as intended.

[AstroStucky]

I have made the changes we discussed in our tag-up today along with some renaming that felt appropriate. Sadly the GitHub diff fails to recognize file similarities again, so I will specify the changes here. Code really did not change, things just got moved around, so a secondary code review is not really necessary.

• arm_action_mixins.py is now mixins.py, and contains only mixin classes.
• trajectory_executor.py is now arm_interface.py and now contains the ArmInterface class.
• Import statements updated as needed.

[anthonykoutroulis]

The code changes/organization look good and the tests work as described.

Quick observation that may be 'as intended': When doing a lock fault during stow, the arm fails to return as described with the console reporting the lock and then Stow action complete. Interestingly, if the faults are unticked after the stow action completes, the arm resumes its attempt to stow (albeit unsuccessfully) without a stow command.

[AstroStucky]

Quick observation that may be 'as intended': When doing a lock fault during stow, the arm fails to return as described with the console reporting the lock and then Stow action complete. Interestingly, if the faults are unticked after the stow action completes, the arm resumes its attempt to stow (albeit unsuccessfully) without a stow command.

I am not sure it is as intended, it's more like undefined behavior as a result of the continue_arm_in_fault flag. I don't think we have decided on what the behavior should be yet, and as Michael pointed out in his earlier comment, doing so should probably be out of scope for this PR.

[anthonykoutroulis]

I suppose that's what I meant by 'intended' 😆. It's out of the scope of this PR.

All good for a merge on my end.

[kmdalal]

The now-empty actions directory needs to be removed, otherwise this looks good! I gave it a quick retest.