plan_route_plugin: waypoints_ relative to target_frame

[facontidavide]

Hi,

this modification of plan_route_plugin is mostly related to code simplification.

1. waypoints will contain poses relative to target_frame_, not stu::_wgs84_frame. This greatly reduces the need for tf_manager_.GetTransform() in many locations of the file, making it more readable.

2. Only inside the method PlanRoute() the conversion from target_frame_ to stu::_wgs84_frame is done.

3. if the latter transform fails, the mnm::PlanRoute is still send, with:

     plan_route.request.header.frame_id = target_frame_;
   

but maybe you do NOT want to do this...

4. Similarly, in Draw(), if this fails

    tf_manager_.GetTransform(stu::_wgs84_frame, target_frame_, transform)
   

a straight line is drawn instead. Personal taste I guess. Since I don't have the PlanRoute server, I am not able to thoughtfully test it, though.

UPDATE: of course I did test 90% of these changes on my own fork, but I am unable to properly check anything related to the Route server

Davide