Behavior simulated drones is different from real drones

[hc825b]

Currently in either cymulator/Drone/goto.py or cym_deivce/cym_waypoint_driver_drone.py

1. the drone periodically publishes reached topic instead of only publishing when reached a waypoint.
2. The drone goes to a waypoint without considering the frame id.

The drone should only publishes to reached in following conditions:

• True: the drone has reached the waypoint.
• False: it is impossible for the drone to reach the waypoint.

The desired way to handle waypoint topic is:

• With frame_id=0, queue the waypoint message
• With frame_id=1, pop and store all previously queued messages as a path and move accordingly

[hc825b]

In ac951fc, reached topic is published only when the drone reach current waypoint.

Still need to handle frame_id