Controllers should die if watchdog fails

[amalnanavati]

Description

This PR adds a new launchfile, demo_feeding.launch.py, that launches all the same nodes as demo.launch.py but also launches a watchdog listener and completely terminates the launchfile if the watchdog fails.

Testing

Pull the code from ada_feeding#48

Setup: Sim

1. Run the dummy FT sensor: ros2 run ada_feeding dummy_ft_sensor.py
2. Run the watchdog: ros2 launch ada_feeding ada_feeding_launch.xml
3. Launch MoveIt: ros2 launch ada_moveit demo_feeding.launch.py sim:=mock

Setup: Real Robot

1. Run the FT sensor: ros2 run forque_sensor_hardware forque_sensor_hardware —ros-args -p host:=xxx.xxx.x.xx
   1. Re-tare the sensor: ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}"
2. Run the watchdog: ros2 launch ada_feeding ada_feeding_launch.xml
3. Launch MoveIt: ros2 launch ada_moveit demo_feeding.launch.py

Tests

• Verify that ada_watchdog_listener_node.py works as expected:

  • [x] Run the dummy FT sensor, the ada_feeding launchfile, the listener node ros2 run ada_watchdog_listener ada_watchdog_listener_node --ros-args --remap ada_watchdog_listener/watchdog:=ada_watchdog/watchdog. Verify that the node stays alive.
  • [x] Kill the dummy FT sensor. Verify that the node dies.
  • [x] Re-start the dummy FT sensor and the listener node. Set the dummy FT sensor to be 0 variance in at least one dimension: ros2 param set /dummy_ft_sensor std '[0.1, 0.1, 0.0, 0.1, 0.1, 0.1]'. Verify that the listener node dies.
  • [x] Re-start the dummy FT sensor and the listener node. Re-tare the F/T sensor ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}". Verify that the listener node dies.
  • [x] Re-start the dummy FT sensor and the listener node and turn the listener node off ros2 service call /ada_watchdog_listener/toggle_watchdog_listener std_srvs/srv/SetBool "{data: false}". Re-tare the F/T sensor ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}". Then, turn the listener node on. Verify that the listener node stays alive.

• Run the setup steps above. Then, turn off the watchdog listener, re-tare the FT sensor, and re-turn on the watchdog listener. Then: Call ros2 action send_goal /MoveAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedback and verify that it succeeds.

  • [x] Sim
  • [x] Real

• Do the above but terminate FT node while the robot is moving, verify that the MoveIt launchfile terminates within 0.1sec.

  • [x] Sim
  • [x] Real

Notes / Discussion Points

1. The watchdog params and remapping for create_action_servers.py are defined in ada_feeding, whereas the same parameters/remapping are also defined in demo_feeding.launch.py. Are we okay with this?
2. EDIT: This has now been addressed. Note that the order of launches becomes important, because we first have to start the watchdog, then start MoveIt, and then populate the planning scene, whereas right now the watchdog and planning scene are launched together. I'll probably modify ada_feeding#48 to have the planning scene node wait until there is a subscriber on the topic, to address this.

[amalnanavati]

Moved the watchdog listener into this repo and re-ran all tests in sim. Will next test in real.

[amalnanavati]

All tested and it works. Will merge.