[ROS2] Improve ordering of launch due to watchdog startup condition

[amalnanavati]

Currently, after ada_feeding is launched, the watchdog fails until the e-stop button is clicked once. Further, ada_moveit currently kills itself if the watchdog is failing. As a result, the launch order is:

1. Launch ada_feeding
2. User clicks watchdog
3. Launch ada_moveit

This is nonideal because we have a user-in-the-loop launch sequence. Ideally, we should be able to launch the code once and then the user pushes the e-stop button (for the watchdog startup condition) and then continues with feeding.

I can think of the following ways to address it:

1. The watchdog_listener that kills the controller starts "off." And should be turned on before any motion. The downside of this is that if the programmer forgets to turn it on because a motion, then that motion is not protected by the watchdog.
2. The watchdog_listener treats startup conditions differently from regular conditions. Specifically, if a startup condition fails it doesn't kill itself. However, this is also dangerous because a programmer could move the robot without it being protected by the watchdog (if the startup conditions haven't passed).
3. We allow the watchdog to take in a parameter for the bash command it should run after the startup conditions succeed. This might be challenging to get to work, because: (a) when running sys comments in Python, does it run in the same process or a different process (the latter is what we want)? (b) what environment variables are set in Python, and does that include all the required environment variables to ros2 launch a node? But even if we get it to work, this feels a bit non-ideal to me since we are allowing someone to pass in an arbitrary bash command in the yaml that our code will blindly execute.

As you can see, all the above approaches have downsides. Thoughts @taylorkf @egordon ?

[egordon]

@amalnanavati

Ugh, if actions were still just topics, we could have the watchdog listener listen on the goal topic and die if both the startup condition isn't met and somebody tries to call the execute trajectory action. But I don't think that introspection is easy (or possible?).

But another possibility is to continuously spam the "stop" command to MoveIt2, thus preventing the robot from moving, until all startup conditions are met.

See https://github.com/personalrobotics/pymoveit2/blob/44cf196da3cbc1d49c480e3c578169d9383f9296/pymoveit2/moveit2.py#L245

This is actually the cleanest way to cancel a trajectory right now.

[amalnanavati]

Change the watchdog to return diagnostic status warn if the startup condition fails. On the watchdog listener side, if a status is warn it spams stop to MoveIt, if a status is error it kills itself, else it keeps listening.

This is to account for the face that from the watchdog listener's perspective, there is nothing that differentiates startup conditions from regular conditions. But there is something that differentiates warn from error. So by norm we can make startup conditions warn, and have the watchdog listener not kill itself on error.