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.

ur_robot_driver/src/ros/robot_state_helper.cpp

@@ -210,9 +210,9 @@ void RobotStateHelper::setModeGoalCallback()
       }
       else
       {
-        result_.success = true;
-        result_.message = "Target mode already active. Nothing to do here.";
-        set_mode_as_.setSucceeded(result_);
+        // There is no transition to do here, but we have to start the program again, if desired.
+        // This happens inside updateRobotState()
+        updateRobotState();
       }
       break;
     case RobotMode::NO_CONTROLLER: