Feature/selective aruco detect

[swaroophs]

Hello!

This PR is adjacently related to this issue that I reported. However it does not directly fix that. While I was debugging the issue, I found that the aruco_detect node was also consuming CPU when the autodock algorithm was idle. Screenshot of htop shown below shows aruco_detect CPU consumption when IDLE:

Ideally, we should not be detecting/looking for aruco markers until an AutoDock action itself is started. But it seems like on default config, the aruco_detect is always on and looking for these markers resulting in high CPU consumption. Looking into this further, the aruco_detect node DOES have a neat service called /enable_detections that lets us enable/disable detections. So without touching the source code for aruco_detect, we will be able to manage the state of these detections from the autodock node itself.

This PR primarily helps implement this with the following changes:

1. Add service client and utility function to set detections
2. Set aruco detections based on autodock state

Some screenshots below from interactive testing with Turtlebot sim:
Below, you can see that aruco_detect consumption has reduced when the autodock is idle:

On startup itself, we disable detections so when the sim is launched with the rviz, we will NOT see any images from the /fiducial_images topic:

[INFO] [1652233347.443160, 0.000000]: Starting AutoDockServer Node
Enable detections response:  Disabled aruco detections.

Below, you can see that aruco_detect is back to normal consumption when autodock is in progress:

Now we start seeing images on the /fiducial_images topic:

Enable detections response:  Enabled aruco detections.
[INFO] [1652233573.711246, 104.431000]: Will attempt with 1 retry

When autodock fails:

[ERROR] [1652233576.152186, 106.457000]: Not detecting two side markers, exit state
[WARN] [1652233576.159298, 106.460000]: Failed to Dock attempt
[WARN] [1652233576.163408, 106.467000]: Attempting retry: 1/1
[INFO] [1652233576.180853, 106.479000]: State is not retry-able
[WARN] [1652233576.187205, 106.482000]: Not able to retry
Enable detections response:  Disabled aruco detections.
[WARN] [1652233576.213065, 106.503000]:  State: [IDLE] | Failed execute Dock Action

When autodock succeeds:

[WARN] [1652233696.103499, 210.448000]:  State: [ACTIVATE_CHARGER] | Docked, activate charger!
[ INFO] [1652233696.111709104, 210.458000000]: ObstacleObserver received autodock state: 6
[WARN] [1652233698.406090, 212.458000]:  State: [ACTIVATE_CHARGER] | Celebration!!
[ INFO] [1652233698.411430833, 212.462000000]: ObstacleObserver received autodock state: 6
[WARN] [1652233698.411304, 212.461000]:  State: [IDLE] | All Success!
[ INFO] [1652233698.422862282, 212.472000000]: ObstacleObserver received autodock state: 1
Enable detections response:  Disabled aruco detections

Note that in rviz, once the docking action is complete, the image will look "frozen". That is because, we disable detections at the end of the action and aruco_detect will stop publishing to the /fiducial_images topic.

Points to ponder:

1. Currently the behavior is the detections are off on startup and are selectively enabled/disabled based on the dock state. Should there be an option to not do this when some sort of debug flag is passed for ease of debugging which in essence will turn off this feature and go back to previous default behavior?
2. There is now extra dependency on the /enable_detections service succeeding. In the event of failure of this service, we rely on the Autodock action's built-in retry mechanism to re-call the service instead of its own retry. I thought it was easier to keep track of the retries this way and conceptually consider that enabling/disabling detections is part of the docking algorithm itself.

[youliangtan]

This feature is really helpful, greatly appreciate your contribution and help in testing this on your robots!! Able to toggle off the aruco_detect greatly reduce the CPU usage during idle. The 'debug mode" recommendation is great to have, so hopefully this can also be included in this pr.

[swaroophs]

@youliangtan I have updated the PR with your recommendations. Try it with the following two launch variations:

Default behavior when debug_mode set to True:
roslaunch autodock_sim tb3_nav_dock_sim.launch

debug_mode set to False that can be used for deployments:
roslaunch autodock_sim tb3_nav_dock_sim.launch debug_mode:=False

Note: Even though it was not in the scope of our original conversation, I used parameter to turn "off" the timer used to publish debug viz markers to visualize in rviz. If for some reason you think we should not be doing this, let me know and I can remove the logic around it.

[youliangtan]

I tested the debug_mode on simulation with the turtlebot. It works well! Thanks again for the contribution