Add error-calculator for localization estimate vs. Gazebo sub location

[hmmwhatsthisdo]

This PR adds a node (localization_harness) that utilizes the newly-published cobalt_sim TF frame generated from Gazebo data (as implemented in #103) and the cobalt TF frame generated using localization-system output (as implemented in PalouseRobosub/robosub#327) and and outputs two topics:

• /localization/error/vector the error as a geometry_msgs::Vector3Stamped
• /localization/error/linear, the above vector's length (so localization-accuracy tests only need to verify "Do the floats published by this topic never exceed x?")

---

This change is 

[skallaher]

TF has a ros specific interface API. It specifically gives back geometry_msgs types.

---

Reviewed 2 of 3 files at r1, 1 of 1 files at r2. Review status: all files reviewed at latest revision, 3 unresolved discussions.

---

launch/gazebo.launch, line 21 at r2 (raw file):

<node name="virtual_serial_port_thruster" pkg="robosub_simulator" type="create_thruster_vsp.sh" />
<node name="simulator_bridge" pkg="robosub_simulator" type="simulator_bridge" required="true" />
<node name="localization_harness" pkg="robosub_simulator" type="localization_harness" required="true" />

I don't know if this should be required for gazebo.

---

src/localization_harness.cpp, line 24 at r2 (raw file):

    ros::Publisher vector_error_pub =
        nh.advertise<geometry_msgs::Vector3Stamped>("localization/error/vector",
        1);

I would prefer that this be aligned with the argument on the line above. I.e.:

....::Vector3Stamped>("localization/error/vector",
                      1);

---

src/localization_harness.cpp, line 38 at r2 (raw file):

    {
        isTransformAvailable = tflr.waitForTransform(tflr.resolve("cobalt_sim"),
            tflr.resolve("cobalt"), ros::Time::now(), ros::Duration(300.0));

Similar here. It's easier to read if all of the args are lined up. Line 43 and 53 as well. I also don't believe you need to call resolve unless you're dealing with outdated tf prefixes.

---

Comments from Reviewable

[hmmwhatsthisdo]

That guide appears to be for TF2, which doesn't appear to implement waitForTransform for some reason (unless I'm simply not seeing it anywhere).

---

Review status: all files reviewed at latest revision, 3 unresolved discussions.

---

launch/gazebo.launch, line 21 at r2 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

I don't know if this should be required for gazebo.

Would changing it to non-required be acceptable, or would you like it removed from the launchfile entirely?

---

Comments from Reviewable

[hmmwhatsthisdo]

Review status: 2 of 3 files reviewed at latest revision, 3 unresolved discussions.

---

src/localization_harness.cpp, line 24 at r2 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

I would prefer that this be aligned with the argument on the line above. I.e.: ``` ....::Vector3Stamped>("localization/error/vector", 1); ```

Done.

---

src/localization_harness.cpp, line 38 at r2 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

Similar here. It's easier to read if all of the args are lined up. Line 43 and 53 as well. I also don't believe you need to call resolve unless you're dealing with outdated tf prefixes.

Done.

---

Comments from Reviewable

[skallaher]

TF2 is the most recent version and is the one you should use. Here's TF2's equivalent to waitForTransform: http://wiki.ros.org/tf2/Tutorials/tf2%20and%20time%20%28C%2B%2B%29#Wait_for_transforms

---

Reviewed 1 of 1 files at r3. Review status: all files reviewed at latest revision, 2 unresolved discussions.

---

launch/gazebo.launch, line 21 at r2 (raw file):

Previously, hmmwhatsthisdo wrote…

Would changing it to non-required be acceptable, or would you like it removed from the launchfile entirely?

That would be fine.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

    ros::NodeHandle nh;

    ros::Rate publishRate(200.0);

This is an extremely fast rate.

---

Comments from Reviewable

[hmmwhatsthisdo]

Review status: 1 of 3 files reviewed at latest revision, 3 unresolved discussions.

---

launch/gazebo.launch, line 21 at r2 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

That would be fine.

Done. I've also pushed it into the non-minimal group in the launchfile.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

This is an extremely fast rate.

I've halved the rate. rqt shows /tf publishing at ~80-100Hz, so I'd like to match that if at-all possible. If need be, I may add in a listener for /tf that only does the lookupTransform if a transform comes in matching cobalt or cobalt_sim

---

src/localization_harness.cpp, line 56 at r4 (raw file):

                                                vector_error_msg.vector.y,
                                                vector_error_msg.vector.z)
                                                .length();

FYI: I only did this because geometry_msgs::Vector3/::Vector3Stamped appears to be a container class only and doesn't expose a method for calculating length. (I could also write a formula to calculate it manually, but I wasn't sure if that would be necessary)

---

Comments from Reviewable

[skallaher]

Reviewed 2 of 2 files at r4. Review status: all files reviewed at latest revision, 2 unresolved discussions.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

Previously, hmmwhatsthisdo wrote…

I've halved the rate. `rqt` shows `/tf` publishing at ~80-100Hz, so I'd like to match that if at-all possible. If need be, I may add in a listener for `/tf` that only does the `lookupTransform` if a transform comes in matching `cobalt` or `cobalt_sim`

The fastest other node we have is the control system at 50hz. Anything faster is not necessarily helpful and shouldn't cause any other nodes to suffer. The localization system itself is only running at 30hz.

---

src/localization_harness.cpp, line 38 at r4 (raw file):

        "Waiting for available transformation from cobalt to cobalt_sim...");

    while (nh.ok())

ros::ok() is the more standard way of checking for shutdown.

---

Comments from Reviewable

[hmmwhatsthisdo]

Review status: 2 of 3 files reviewed at latest revision, 2 unresolved discussions.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

The fastest other node we have is the control system at 50hz. Anything faster is not necessarily helpful and shouldn't cause any other nodes to suffer. The localization system itself is only running at 30hz.

Running the simulator in headless mode shows the harness using slightly less CPU time (load average of about 0.15 threads) than the control system when running at 100Hz. I can lower it, but my intent was to have it publishing error data based on the most accurate positional data available (from ROS's perspective) at any given time. I'm concerned that lowering the rate might induce some unintentional smoothing on the error measurement(s).

If I were to have the node listen directly to /tf and only lookup/process/publish when an updated frame becomes available on the TF tree, would this be more preferable?

---

src/localization_harness.cpp, line 38 at r4 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

`ros::ok()` is the more standard way of checking for shutdown.

Fixed - the tutorial was using nh.ok(), so I figured the two were interchangeable.

---

Comments from Reviewable

[skallaher]

Reviewed 1 of 1 files at r5. Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

Previously, hmmwhatsthisdo wrote…

Running the simulator in headless mode shows the harness using slightly less CPU time (load average of about 0.15 threads) than the control system when running at 100Hz. I can lower it, but my intent was to have it publishing error data based on the most accurate positional data available (from ROS's perspective) at any given time. I'm concerned that lowering the rate might induce some unintentional smoothing on the error measurement(s). If I were to have the node listen directly to `/tf` and only lookup/process/publish when an updated frame becomes available on the TF tree, would this be more preferable?

It's mostly because there is no need to push out data faster than the localization engine can update. I would also recommend making this rate a parameter. 100 Hz is just plain excessive for how long you'll be running things. If a run was something like 3 seconds or so, I might see running the node this fast. @irwineffect, what is your opinion.

---

Comments from Reviewable

[hmmwhatsthisdo]

Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

src/localization_harness.cpp, line 15 at r3 (raw file):

Previously, skallaher (Sean Kallaher) wrote…

It's mostly because there is no need to push out data faster than the localization engine can update. I would also recommend making this rate a parameter. 100 Hz is just plain excessive for how long you'll be running things. If a run was something like 3 seconds or so, I might see running the node this fast. @irwineffect, what is your opinion.

@skallaher @irwineffect I'll drop this to 30hz for now - if it ends up that the harness node is producing poor data, it may be worth having a direct subscription to /tf performing the callback.

---

Comments from Reviewable

[skallaher]

Reviewed 1 of 1 files at r6. Review status: all files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[skallaher]

---

Review status: :shipit: all files reviewed at latest revision, all discussions resolved, all commit checks successful.

---

Comments from Reviewable

[hmmwhatsthisdo]

BTW @skallaher: When discussions are resolved on a PR, do you prefer that we wait for you or James to do the rebase/merge, or should we just make it happen ourselves via Reviewable?

---

Review status: :shipit: all files reviewed at latest revision, all discussions resolved, all commit checks successful.

---

Comments from Reviewable

[skallaher]

FYI @hmmwhatsthisdo, if you would like to do the rebase to put your branch on top of master, you're welcome to. However, only @irwineffect and I have permissions to actually push updates to master. I typically wait for a bit before merging anything that I've approved to see if anyone else wants to put in their two cents while they can. You should not be doing any merging for RoboSub.

---

Review status: :shipit: all files reviewed at latest revision, all discussions resolved, all commit checks successful.

---

Comments from Reviewable

[hmmwhatsthisdo]

Ah, OK - I saw the "Rebase and Retain Branch" button on Reviewable light up now that discussions have resolved and wasn't sure if we were supposed to be doing our own merging via Reviewable or somesuch.

---

Review status: :shipit: all files reviewed at latest revision, all discussions resolved, all commit checks successful.

---

Comments from Reviewable