From f8e97ef1b1a6c5f41b010b3aab53120bf8c7c7c9 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Tue, 16 Mar 2021 00:37:59 -0400 Subject: [PATCH] Create robot object before changing mode --- stretch_core/nodes/stretch_driver | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 7627780..44f1ddc 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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)