def __init__(self, argv):
        self.node_name = "ObjectMeasurer"
        rospy.init_node(self.node_name)

        # What we do during shutdown
        rospy.on_shutdown(self.cleanup)

        # Create the cv_bridge object
        self.bridge = CvBridge()

        # Subscribe to the camera image topics and set
        # the appropriate callbacks
        self.image_sub = rospy.Subscriber(argv[1], Image, self.image_callback)
        self.image_pub = rospy.Publisher(
            "%s/ObjectMeasurer" % (argv[1]), Image, queue_size=10)
        rospy.loginfo("Waiting for image topics...")

        # Initialization of the 'pixels per metric variable'
        self.pixelsPerMetric = None 

def __init__(self, args):
        self.node_name = "MaskPlantTray"
        rospy.init_node(self.node_name)

        # What we do during shutdown
        rospy.on_shutdown(self.cleanup)

        # Create the cv_bridge object
        self.bridge = CvBridge()

        # Subscribe to the camera image topics and set
        # the appropriate callbacks
        self.image_sub = rospy.Subscriber(args[1], Image, self.image_callback)
        self.image_pub = rospy.Publisher(
            "%s/MaskPlantTray" % (args[1]), Image, queue_size=10)

        rospy.loginfo("Waiting for image topics...") 

def __init__(self, args):
        self.node_name = "ImgMerger"
        rospy.init_node(self.node_name)

        # What we do during shutdown
        rospy.on_shutdown(self.cleanup)

        # Create the cv_bridge object
        self.bridge = CvBridge()

        self.frame_img1 = np.zeros((1280, 1024), np.uint8)
        self.frame_img2 = np.zeros((1280, 1024), np.uint8)

        # Subscribe to the camera image topics and set
        # the appropriate callbacks
        self.image_sub_first_image = rospy.Subscriber(
            args[1], Image, self.image_callback_img1)
        self.image_sub_second_image = rospy.Subscriber(
            args[2], Image, self.image_callback_img2)

        self.image_pub = rospy.Publisher(args[3], Image, queue_size=10)

        rospy.loginfo("Waiting for image topics...") 

def main():
    """RSDK Head Example: Wobbler

    Nods the head and pans side-to-side towards random angles.
    Demonstrates the use of the intera_interface.Head class.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_head_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    print("Wobbling... ")
    wobbler.wobble()
    print("Done.") 

def main():
    """Intera RSDK Joint Velocity Example: Wobbler

    Commands joint velocities of randomly parameterized cosine waves
    to each joint. Demonstrates Joint Velocity Control Mode.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_velocity_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    wobbler.wobble()

    print("Done.") 

def __init__(self):
        self.process_list = []
        self.process_config = rospy.get_param("~processes")
        self.create_processes()

        rospy.on_shutdown(self.clean_ros_processes)

        self.process_state_publish_rate = rospy.get_param("~process_state_publish_rate")

        self.process_state_publisher = rospy.Publisher(
            '/niryo_one/rpi/process_state', ProcessState, queue_size=1)

        rospy.Timer(rospy.Duration(1.0 / self.process_state_publish_rate), self.publish_process_state)

        self.manage_process_server = rospy.Service(
            '/niryo_one/rpi/manage_process', ManageProcess, self.callback_manage_process)

        self.start_init_processes()
        # self.start_all_processes() 

def __init__(self):
        self.node_name = "cv_bridge_demo"
        
        rospy.init_node(self.node_name)
        
        # What we do during shutdown
        rospy.on_shutdown(self.cleanup)
        
        # Create the OpenCV display window for the RGB image
        self.cv_window_name = self.node_name

        # Create the cv_bridge object
        self.bridge = CvBridge()
        
        # Subscribe to the camera image and depth topics and set
        # the appropriate callbacks

        rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
        
        rospy.Timer(rospy.Duration(0.03), self.show_img_cb)
        rospy.loginfo("Waiting for image topics...") 

def __init__(self):
        self.node_name = "cv_bridge_demo"
        
        rospy.init_node(self.node_name)
        
        # What we do during shutdown
        rospy.on_shutdown(self.cleanup)
        
        # Create the OpenCV display window for the RGB image
        self.cv_window_name = self.node_name

        # Create the cv_bridge object
        self.bridge = CvBridge()
        
        # Subscribe to the camera image and depth topics and set
        # the appropriate callbacks

        rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
        
        rospy.Timer(rospy.Duration(0.03), self.show_img_cb)
        rospy.loginfo("Waiting for image topics...") 

def move_gripper(limb, action):
    # initialize interfaces
    print("Getting robot state...")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    gripper = None
    original_deadzone = None
    
    def clean_shutdown():
        if gripper and original_deadzone:
            gripper.set_dead_zone(original_deadzone)
        print("Finished.")
    
    try:
        gripper = intera_interface.Gripper(limb + '_gripper')
    except (ValueError, OSError) as e:
        rospy.logerr("Could not detect an electric gripper attached to the robot.")
        clean_shutdown()
        return
    rospy.on_shutdown(clean_shutdown)

    original_deadzone = gripper.get_dead_zone()
    # WARNING: setting the deadzone below this can cause oscillations in
    # the gripper position. However, setting the deadzone to this
    # value is required to achieve the incremental commands in this example
    gripper.set_dead_zone(0.001)
    rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
    rospy.loginfo("Enabling robot...")
    rs.enable()
    print("Controlling grippers.")
    if (action == "open"):
        gripper.open()
        rospy.sleep(1.0)
        print("Opened grippers")
    elif (action == "close"):
        gripper.close()
        rospy.sleep(1.0)
        print("Closed grippers")
    
    # force shutdown call if caught by key handler
    rospy.signal_shutdown("Example finished.") 

def start_server(limb, rate, mode, valid_limbs):
    rospy.loginfo("Initializing node... ")
    rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
                        mode, "" if limb == 'all_limbs' else "_" + limb,))

    rospy.wait_for_service("/ExternalTools/right/PositionKinematicsNode/IKService")

    rospy.loginfo("Initializing joint trajectory action server...")
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
    config_module = "intera_interface.cfg"
    if mode == 'velocity':
        config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"])
    elif mode == 'position':
        config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"])
    else:
        config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"])
    cfg = importlib.import_module('.'.join([config_module,config_name]))
    dyn_cfg_srv = Server(cfg, lambda config, level: config)
    jtas = []
    if limb == 'all_limbs':
        for current_limb in valid_limbs:
            jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv,
                                                    rate, mode))
    else:
        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))


    def cleanup():
        for j in jtas:
            j.clean_shutdown()

    rospy.on_shutdown(cleanup)
    rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
    rospy.spin() 

def __init__(self):

        # set up openrave
        self.env = rave.Environment()
        self.env.StopSimulation()
        self.env.Load("robots/pr2-beta-static.zae")  # todo: use up-to-date urdf
        self.robot = self.env.GetRobots()[0]

        self.joint_listener = TopicListener("/joint_states", sm.JointState)

        # rave to ros conversions
        joint_msg = self.get_last_joint_message()
        ros_names = joint_msg.name
        inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
        self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1)  # ros joints inds with matching rave joint
        self.rave_inds = inds_ros2rave[self.good_ros_inds]  # openrave indices corresponding to those joints
        self.update_rave()

        self.larm = Arm(self, "l")
        self.rarm = Arm(self, "r")
        self.lgrip = Gripper(self, "l")
        self.rgrip = Gripper(self, "r")
        self.head = Head(self)
        self.torso = Torso(self)
        self.base = Base(self)

        rospy.on_shutdown(self.stop_all) 

def main():
    """Save / Load EndEffector Config utility

    Read current config off of ClickSmart and save to file.
    Or load config from file and reconfigure and save it to ClickSmart device.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)

    parser.add_argument(
        '-s', '--save', metavar='PATH',
        help='save current EE config to given file'
    )
    parser.add_argument(
        '-l', '--load', metavar='PATH',
        help='load config from given file onto EE'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node('ee_config_editor', anonymous=True)

    ee = intera_interface.get_current_gripper_interface()
    if not ee:
        rospy.logerr("Could not detect an attached EndEffector!")
        return

    if args.save:
        rospy.loginfo("Saving EE config to {}".format(args.save))
        save_config(ee, args.save)

    if args.load:
        rospy.loginfo("Loading config and writing config to ClickSmart from {}".format(args.load))
        load_config(ee, args.load)

    def clean_shutdown():
        print("\nExiting example...")

    rospy.on_shutdown(clean_shutdown) 

def start_server(limb, rate, mode, valid_limbs):
    rospy.loginfo("Initializing node... ")
    rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
                        mode, "" if limb == 'all_limbs' else "_" + limb,))

    rospy.loginfo("Initializing joint trajectory action server...")
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
    config_module = "intera_interface.cfg"
    if mode == 'velocity':
        config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"])
    elif mode == 'position':
        config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"])
    else:
        config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"])
    cfg = importlib.import_module('.'.join([config_module,config_name]))
    dyn_cfg_srv = Server(cfg, lambda config, level: config)
    jtas = []
    if limb == 'all_limbs':
        for current_limb in valid_limbs:
            jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv,
                                                    rate, mode))
    else:
        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))


    def cleanup():
        for j in jtas:
            j.clean_shutdown()

    rospy.on_shutdown(cleanup)
    rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
    rospy.spin() 

def main():
    """SDK Joint Position Waypoints Example

    Records joint positions each time the navigator 'OK/wheel'
    button is pressed.
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
    will begin playing back in a loop.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.add_argument(
        '-s', '--speed', default=0.3, type=float,
        help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
    )
    parser.add_argument(
        '-a', '--accuracy',
        default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
        help='joint position accuracy (rad) at which waypoints must achieve'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_joint_position_waypoints", anonymous=True)

    waypoints = Waypoints(args.speed, args.accuracy)

    # Register clean shutdown
    rospy.on_shutdown(waypoints.clean_shutdown)

    # Begin example program
    waypoints.record()
    waypoints.playback() 

def __init__(self):
        rospy.init_node('sound_server', anonymous=False)
        rospy.on_shutdown(self._shutdown)
        while (rospy.get_rostime() == 0.0):
            pass
        rospy.Subscriber("/sound_server/speech_synth", String, self.cb_speech_synth)
        rospy.Subscriber("/sound_server/play_sound_file", String, self.cb_play_sound_file)
        self.pub_status = rospy.Publisher("/sound_server/status", String)
        self.s = mirage_sound_out(festival_cache_path="/home/ubuntu/Music/mirage_engrams/festival_cache/", sound_effects_path="/home/ubuntu/Music/mirage_engrams/sound_effects/")
        rospy.sleep(2.0) # Startup time.
        rospy.loginfo("mirage_sound_out ready")
        rospy.spin() 

def get_python_code_from_xml(self, xml):
        return self.blockly_generator.get_generated_python_code(xml)

    # !! Need to call this with rospy.on_shutdown !! 

def __init__(self):
        self.wifi_manager_enabled = rospy.get_param("~wifi_manager_enabled")
        self.launch_ros_processes = rospy.get_param("~launch_ros_processes")
        self.modbus_server_enabled = rospy.get_param("~modbus_server_enabled")
        self.modbus_server_address = rospy.get_param("~modbus_server_address")
        self.modbus_server_port = rospy.get_param("~modbus_server_port")
        self.niryo_one_ros_setup = None

        if self.launch_ros_processes:
            self.niryo_one_ros_setup = NiryoOneRosSetup()
            rospy.sleep(10)  # let some time for other processes to launch (does not affect total launch time)

        # Start wifi manager
        if self.wifi_manager_enabled:
            self.wifi_manager = WifiConnectionManager()

        self.fans_manager = FansManager()
        self.ros_log_manager = RosLogManager()
        self.shutdown_manager = ShutdownManager()
        self.led_manager = LEDManager()
        self.niryo_one_button = NiryoButton()
        self.digital_io_panel = DigitalIOPanel()
        self.motor_debug = MotorDebug()

        # Start Modbus server
        if self.modbus_server_enabled:
            self.modbus_server = ModbusServer(self.modbus_server_address, self.modbus_server_port)
            rospy.on_shutdown(self.modbus_server.stop)
            self.modbus_server.start() 

def __init__(self):
        self.pin = BUTTON_GPIO
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        rospy.loginfo("GPIO setup : " + str(self.pin) + " as input")
        rospy.sleep(0.1)

        self.last_state = self.read_value()
        self.consecutive_pressed = 0
        self.activated = True

        self.timer_frequency = 20.0
        self.shutdown_action = False
        self.hotspot_action = False

        self.button_mode = ButtonMode.TRIGGER_SEQUENCE_AUTORUN
        self.change_button_mode_server = rospy.Service(
            "/niryo_one/rpi/change_button_mode", SetInt, self.callback_change_button_mode)
        self.monitor_button_mode_timer = rospy.Timer(rospy.Duration(3.0), self.monitor_button_mode)
        self.last_time_button_mode_changed = rospy.Time.now()

        # Publisher used to send info to Niryo One Studio, so the user can add a move block
        # by pressing the button
        self.save_point_publisher = rospy.Publisher(
            "/niryo_one/blockly/save_current_point", Int32, queue_size=10)

        self.button_state_publisher = rospy.Publisher(
            "/niryo_one/rpi/is_button_pressed", Bool, queue_size=10)

        self.button_timer = rospy.Timer(rospy.Duration(1.0 / self.timer_frequency), self.check_button)
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo("Niryo One Button started") 

def main():
    """RSDK End Effector Position Example: Keyboard Control

    Use your dev machine's keyboard to control end effector positions.

    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_position_keyboard")
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example...")
        if not init_state:
            print("Disabling robot...")
            rs.disable()
    rospy.on_shutdown(clean_shutdown)

    print("Enabling robot... ")
    rs.enable()

    map_keyboard()
    print("Done.") 

def run_task(*_):
    initial_goal = np.array([0.6, -0.1, 0.80])

    rospy.init_node('trpo_real_sawyer_pnp_exp', anonymous=True)

    pnp_env = TheanoEnv(
        PickAndPlaceEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=False))

    rospy.on_shutdown(pnp_env.shutdown)

    pnp_env.initialize()

    env = pnp_env

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

def run_task(*_):
    initial_goal = np.array([0.6, -0.1, 0.80])

    rospy.init_node('trpo_sim_sawyer_pnp_exp', anonymous=True)

    pnp_env = TheanoEnv(
        PickAndPlaceEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=True))

    rospy.on_shutdown(pnp_env.shutdown)

    pnp_env.initialize()

    env = pnp_env

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

def run_task(*_):
    """Run task function."""
    initial_goal = np.array([0.6, -0.1, 0.30])

    rospy.init_node('trpo_real_sawyer_reacher_exp', anonymous=True)

    env = TheanoEnv(
        ReacherEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=False,
            robot_control_mode='position'))

    rospy.on_shutdown(env.shutdown)

    env.initialize()

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

def run_task(*_):
    initial_goal = np.array([0.6, -0.1, 0.80])

    rospy.init_node('trpo_real_sawyer_push_exp', anonymous=True)

    push_env = TheanoEnv(
        PushEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=False))

    rospy.on_shutdown(push_env.shutdown)

    push_env.initialize()

    env = push_env

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

def run_task(*_):
    """Run task function."""
    initial_goal = np.array([0.6, -0.1, 0.40])

    # Initialize moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)

    rospy.init_node('trpo_sim_sawyer_reacher_exp', anonymous=True)

    env = TheanoEnv(
        ReacherEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=True))

    rospy.on_shutdown(env.shutdown)

    env.initialize()

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

def __init__(self, name):
        """
        :param name: the name of the ROS node
        """
        super(NaoqiNode, self).__init__()

        # A distutils.version.LooseVersion that contains the current verssion of NAOqi we're connected to
        self.__naoqi_version = None
        self.__name = name

        ## NAOqi stuff
        # dict from a modulename to a proxy
        self.__proxies = {}

        # Initialize ros, before getting parameters.
        rospy.init_node(self.__name)

        # If user has set parameters for ip and port use them as default
        default_ip = rospy.get_param("~pip", "127.0.0.1")
        default_port = rospy.get_param("~pport", 9559)

        # get connection from command line:
        from argparse import ArgumentParser
        parser = ArgumentParser()
        parser.add_argument("--pip", dest="pip", default=default_ip,
                          help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP")
        parser.add_argument("--pport", dest="pport", default=default_port, type=int,
                          help="port of parent broker. Default is 9559.", metavar="PORT")

        import sys
        args, unknown = parser.parse_known_args(args=rospy.myargv(argv=sys.argv)[1:])
        self.pip = args.pip
        self.pport = args.pport

        ## ROS stuff
        self.__stop_thread = False
        # make sure that we unregister from everything when the module dies
        rospy.on_shutdown(self.__on_ros_shutdown) 

def __init__(self):
        self.be = None
        Logger.initialize()
        self._tracked_imports = list()
        # hide SMACH transition log spamming
        smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr)

        # prepare temp folder
        self._tmp_folder = tempfile.mkdtemp()
        sys.path.append(self._tmp_folder)
        rospy.on_shutdown(self._cleanup_tempdir)

        # prepare manifest folder access
        self._behavior_lib = BehaviorLibrary()

        # prepare communication
        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'
        self._pub = ProxyPublisher({
            self.feedback_topic: CommandFeedback,
            'flexbe/heartbeat': Empty
        })
        self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
        self._execute_heartbeat()

        # listen for new behavior to start
        self._running = False
        self._run_lock = threading.Lock()
        self._switching = False
        self._switch_lock = threading.Lock()
        self._sub = ProxySubscriberCached()
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        rospy.sleep(0.5)  # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') 

def on_shutdown(self):
        for serial_proxy in self.serial_proxies.values():
            serial_proxy.disconnect() 

def init_node(self) :
		rospy.init_node('GazeboDomainRandom_node', anonymous=False)#, xmlrpc_port=self.port)#,tcpros_port=self.port)
		rospy.on_shutdown(self.close) 

def __init__(self):
        rospy.on_shutdown(self.on_shutdown)
        self.update_rate = rospy.get_param("~update_rate", 10.0)
        self.sensor_frame_id = rospy.get_param("~sensor_frame_id", "respeaker_base")
        self.doa_xy_offset = rospy.get_param("~doa_xy_offset", 0.0)
        self.doa_yaw_offset = rospy.get_param("~doa_yaw_offset", 90.0)
        self.speech_prefetch = rospy.get_param("~speech_prefetch", 0.5)
        self.speech_continuation = rospy.get_param("~speech_continuation", 0.5)
        self.speech_max_duration = rospy.get_param("~speech_max_duration", 7.0)
        self.speech_min_duration = rospy.get_param("~speech_min_duration", 0.1)
        self.main_channel = rospy.get_param('~main_channel', 0)
        suppress_pyaudio_error = rospy.get_param("~suppress_pyaudio_error", True)
        #
        self.respeaker = RespeakerInterface()
        self.respeaker_audio = RespeakerAudio(self.on_audio, suppress_error=suppress_pyaudio_error)
        self.speech_audio_buffer = str()
        self.is_speeching = False
        self.speech_stopped = rospy.Time(0)
        self.prev_is_voice = None
        self.prev_doa = None
        # advertise
        self.pub_vad = rospy.Publisher("is_speeching", Bool, queue_size=1, latch=True)
        self.pub_doa_raw = rospy.Publisher("sound_direction", Int32, queue_size=1, latch=True)
        self.pub_doa = rospy.Publisher("sound_localization", PoseStamped, queue_size=1, latch=True)
        self.pub_audio = rospy.Publisher("audio", AudioData, queue_size=10)
        self.pub_speech_audio = rospy.Publisher("speech_audio", AudioData, queue_size=10)
        self.pub_audios = {c:rospy.Publisher('audio/channel%d' % c, AudioData, queue_size=10) for c in self.respeaker_audio.channels}
        # init config
        self.config = None
        self.dyn_srv = Server(RespeakerConfig, self.on_config)
        # start
        self.speech_prefetch_bytes = int(
            self.speech_prefetch * self.respeaker_audio.rate * self.respeaker_audio.bitdepth / 8.0)
        self.speech_prefetch_buffer = str()
        self.respeaker_audio.start()
        self.info_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate),
                                      self.on_timer)
        self.timer_led = None
        self.sub_led = rospy.Subscriber("status_led", ColorRGBA, self.on_status_led) 

def on_shutdown(self):
        try:
            self.respeaker.close()
        except:
            pass
        finally:
            self.respeaker = None
        try:
            self.respeaker_audio.stop()
        except:
            pass
        finally:
            self.respeaker_audio = None