def __init__()

in robots/LoCoBot/locobot_control/scripts/locobot_control/teleoperation.py [0:0]

• 61 lines of code
• 6 McCabe index (conditional complexity)

    def __init__(self, cfg_file="teleop.yaml"):

        # Load config file
        dir_path = os.path.dirname(os.path.realpath(__file__))
        root_path = os.path.dirname(os.path.dirname(dir_path))
        cfg_path = os.path.join(root_path, "config", cfg_file)
        with open(cfg_path, "r") as f:
            self.cfg = yaml.load(f)
        arm_config = {"control_mode": "position", "use_moveit": False}

        self.use_arm = rospy.get_param("use_arm")
        self.use_camera = rospy.get_param("use_camera")
        self.use_base = rospy.get_param("use_base")
        self.use_gripper = self.use_arm

        self.bot = Robot(
            "locobot",
            use_arm=self.use_arm,
            use_base=self.use_base,
            use_camera=self.use_camera,
            use_gripper=self.use_gripper,
            arm_config=arm_config,
        )

        # Subscribers
        rospy.Subscriber(self.cfg["ROSTOPIC_TELEOP"], String, self._callback_cmd_vel)
        rospy.Subscriber(
            self.cfg["ROSTOPIC_HARDWARE_STATUS"], Bool, self._callback_hardware_status
        )

        # Publishers
        self.pub_stop = rospy.Publisher(
            self.cfg["ROSTOPIC_STOP_EXECUTION"], Empty, queue_size=1
        )
        self.pub_stop_j4 = rospy.Publisher(
            self.cfg["ROSTOPIC_STOP_EXECUTION_J4"], Empty, queue_size=1
        )
        self.pub_stop_j5 = rospy.Publisher(
            self.cfg["ROSTOPIC_STOP_EXECUTION_J5"], Empty, queue_size=1
        )
        self.base_cmd_pub = rospy.Publisher(
            self.cfg["ROSTOPIC_BASE_COMMAND"], Twist, queue_size=1
        )
        self.safety_status_pub = rospy.Publisher(
            self.cfg["ROSTOPIC_SAFETY_STATUS"], Bool, queue_size=1
        )

        # Services
        self.joint_cmd_srv = rospy.ServiceProxy(
            self.cfg["ROSTOPIC_JOINT_COMMAND"], JointCommand
        )

        # Initialize arm, gripper and pan-tilt camera
        if self.use_arm:
            # set arm to a good pose
            # if the arm starts from the rest pose,
            # some teleops might break the arm
            self.bot.arm.set_joint_positions(
                np.array([0, -0.14, 0.76, 1.07, 0]), plan=False
            )
        # NOTE - Do not remove the following steps!
        if self.use_gripper:
            self.bot.gripper.reset()
        if self.use_camera:
            self.bot.camera.set_pan(self.cfg["START_PAN"])
            self.bot.camera.set_tilt(self.cfg["START_TILT"])

        self.cmd = None
        self.status = True
        self.cmd_prev = None
        self.is_robot_moving = False
        self.base_linear_speed = 0.05
        self.base_angular_speed = 0.5
        self.start_time = time.time()

        self.target_alpha = 0.0
        self.update_alpha()
        self.trans, _, _ = self.bot.arm.pose_ee

        rospy.loginfo("Teleop server ready to accept requests...")