Check usage of `goal_handle.accepted`

[hello-binit]

With ROS2 goal handles, goal_handle.accepted can be misleading. Even if the goal was aborted or cancelled, goal_handle.accepted may be True. As @hello-robot-shehab discovered, a more reliable check is goal_handle.status == GoalStatus.STATUS_SUCCEEDED. Not sure if it's ROS2's spec that goal_handle.status is updated behind the scenes, but goal_handle.accepted isn't.

We use goal_handle.accepted throughout the driver nodes, presumably to check the trajectory is not aborted. We should review our usage and confirm it behaves as expected.