[WIP] Use tf2_transform data to get current robot pose in NavFn

[crdelsey]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	fixes #595
Primary OS tested on	Ubuntu
Robotic platform tested on	Turtlebot 3

---

Description of contribution in a few bullet points

• I piggy-backed the current pose from costmap onto the GetCostmap service request in World Model
• I changed timeout values to be in nanoseconds. std::chrono will let you auto-convert from larger time intervals to smaller, but not the other way
• I added a timeout on the GetCostmap service call that NavFn makes

Rationale

I tried to make a standalone version of the getCurrentPose function, but it had so many dependencies, that it seemed the wrong approach to solving the problem. All those dependencies were already available in costmap, and it seems reasonable to ask the world model/costmap where the robot is; it's sort of the equivalent of a "You are here" indicator on a real map.

I was going to add a separate service call to World Model to get the robot pose, but we currently have latency issues with service calls. Instead, I added the pose to the GetCostmap service call. It seems unlikely you will get the costmap without also needing to know where you are on the map at the same time, so it seemed reasonable to combine them until our service call latency issues can be fixed.

NOTE
Updated to use a separate service to get the pose. Need to target this to the lifecycle branch still.

[mkhansenbot]

I don't agree with adding the pose to the existing GetCostmap service. I think it would be better to add a "GetPose" service instead. I realize that services are somewhat flaky but I don't want to combine two data structures into one without being clear. I guess the other option would be to add a new service "GetLocation" which returns both. I realize that may be semantics, but "GetCostmap" should only return a costmap object.

[crdelsey]

I'll try adding another service to world model to get current pose, and see how it goes. If it hurts the user experience too much, I'll go back to this version and rename it.

[crdelsey]

Superceeded by #793