From fb4ba62dbbd6b16fd1fba1be13d717e15b928787 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Tue, 16 Mar 2021 00:39:55 -0400 Subject: [PATCH] Robot object is created before changing mode --- stretch_core/nodes/stretch_driver | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 54d453b..5e59139 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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)