Merge pull request #58 from BFALacerda/hydro-devel

cancelling action properly
making sure robot gets to the correct pose on goal waypoint
replanning added for unexpected state transitions
handling situations where no message is published in /current_node
fixing stupid bug
making sure the robot doesnt get stuck in nav loops between waypoints
outputting succeeded immediately when already in goal waypoint and top_nav also outputs succeeded immediately
Output changes
Changes for local testing.
Ensuring mdp planner shuts down when asked.
Updated test executor to match mdp expectations, so now uses monitored navigation.
Added a stricter cancel for navigation and execution.

This new version does not wait to receive a callback from the cancelled action server. This is dangerous in that the next task may start while the previous task is still ending, but there isn't a huge problem with this in our current tasks. A better solution would be to wait a bit, then give up on waiting for the callback, but this is hard in the current design. Probably needs to be reimplemented as a state machine to make this cleaner.
Building FSM executor
First pass of executor based on smach working.
Added cancellation timeout.

This also checks if we get late preempt responses.
Nav prempt working with concurrence.
Working preempts on action too.

Seems clean and robust for now.
Logging working from state machine now.
Fixed minor scheduling issues.

1) Made service calls thread safe.
2) Fixed order of calls in cancellation
3) Removed blocking assumption in demand task in scheduler
4) Changed bounding of tasks based on current execution time.