Always go through updateRobotState function in goal callback (#99)
Always go through updateRobotState function in goal callback (#99)

When robot is already in the target mode (safety- and robot mode) and the set_mode
action is called with requesting to start the program afterwards, the program
did not start as the robot already was at the desired state.
However, e.g. after a protective stop that is resolved by hand (e.g. when driving
into joint limits) users expected to call that action to restart the robot
again.

With this change, we do the usual check whether to start the program again.
This way, this action can always be used to make sure the robot is running with
the program correctly.
UniversalRobots/Universal_Robots_Client_Library,UniversalRobots/Universal_Robots_Client_Library