Create robot object before changing mode

stretch_core/nodes/stretch_driver

@@ -491,6 +491,9 @@ class StretchBodyNode:
 
         rospy.loginfo("{0} started".format(self.node_name))
 
+        self.robot = rb.Robot()
+        self.robot.startup()
+
         mode = rospy.get_param('~mode', "position")
         rospy.loginfo('mode = ' + str(mode))
         if mode == "position":
@@ -574,12 +577,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()
-
-        # TODO: check with the actuators to see if calibration is required
-        #self.calibrate()
-
         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)