Robot object is created before changing mode

stretch_core/nodes/stretch_driver

@@ -374,6 +374,11 @@ class StretchBodyNode:
 
         rospy.loginfo("{0} started".format(self.node_name))
 
+        self.robot = rb.Robot()
+        self.robot.startup()
+        if not self.robot.is_calibrated():
+            rospy.logwarn("Robot isn't homed!")
+
         mode = rospy.get_param('~mode', "position")
         rospy.loginfo('mode = ' + str(mode))
         if mode == "position":
@@ -457,11 +462,6 @@ class StretchBodyNode:
         self.odom_frame_id = 'odom'
         rospy.loginfo("{0} odom_frame_id = {1}".format(self.node_name, self.odom_frame_id))
 
-        self.robot = rb.Robot()
-        self.robot.startup()
-        if not self.robot.is_calibrated():
-            rospy.logwarn("Robot isn't homed!")
-
         self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
 
         command_base_velocity_and_publish_joint_state_rate = rospy.Rate(self.joint_state_rate)