def __init__(self):
        rospy.loginfo("Tianbot Racecar JoyTeleop Initializing...")
        self._twist = Twist()
        self._twist.linear.x = 1500
        self._twist.angular.z = 90
        self._deadman_pressed = False
        self._zero_twist_published = False

        self._cmd_vel_pub = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5)
        self._joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
        self._timer = rospy.Timer(rospy.Duration(0.05), self.joystick_controller)
        self._axis_throttle = 1

        _joy_mode = rospy.get_param("~joy_mode", "D").lower()
        if _joy_mode == "d":
            self._axis_servo = 2
        if _joy_mode == "x":
            self._axis_servo = 3

        self._throttle_scale = rospy.get_param("~throttle_scale", 0.5)
        self._servo_scale = rospy.get_param("~servo_scale", 1) 

def __init__(self):

        # Tf listener (position + rpy) of end effector tool
        self.position = [0, 0, 0]
        self.rpy = [0, 0, 0]
        self.tf_listener = tf.TransformListener()

        # State publisher
        self.niryo_one_robot_state_publisher = rospy.Publisher(
            '/niryo_one/robot_state', RobotState, queue_size=5)

        # Get params from rosparams
        rate_tf_listener = rospy.get_param("/niryo_one/robot_state/rate_tf_listener")
        rate_publish_state = rospy.get_param("/niryo_one/robot_state/rate_publish_state")

        rospy.Timer(rospy.Duration(1.0 / rate_tf_listener), self.get_robot_pose)
        rospy.Timer(rospy.Duration(1.0 / rate_publish_state), self.publish_state)

        rospy.loginfo("Started Niryo One robot state publisher") 

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):
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)

        lock = Lock()

        self.publish_io_state_frequency = rospy.get_param("~publish_io_state_frequency")
        self.digitalIOs = [DigitalPin(lock, GPIO_1_A, GPIO_1_A_NAME), DigitalPin(lock, GPIO_1_B, GPIO_1_B_NAME),
                           DigitalPin(lock, GPIO_1_C, GPIO_1_C_NAME),
                           DigitalPin(lock, GPIO_2_A, GPIO_2_A_NAME), DigitalPin(lock, GPIO_2_B, GPIO_2_B_NAME),
                           DigitalPin(lock, GPIO_2_C, GPIO_2_C_NAME),
                           DigitalPin(lock, SW_1, SW_1_NAME, mode=GPIO.OUT),
                           DigitalPin(lock, SW_2, SW_2_NAME, mode=GPIO.OUT)]

        self.digital_io_publisher = rospy.Publisher('niryo_one/rpi/digital_io_state', DigitalIOState, queue_size=1)
        rospy.Timer(rospy.Duration(1.0 / self.publish_io_state_frequency), self.publish_io_state)

        self.get_io_server = rospy.Service('niryo_one/rpi/get_digital_io', GetDigitalIO, self.callback_get_io)
        self.set_io_mode_server = rospy.Service('niryo_one/rpi/set_digital_io_mode', SetDigitalIO,
                                                self.callback_set_io_mode)
        self.set_io_state_server = rospy.Service('niryo_one/rpi/set_digital_io_state', SetDigitalIO,
                                                 self.callback_set_io_state) 

def __init__(self):
        self.log_size_treshold = rospy.get_param("~ros_log_size_treshold")
        self.log_path = rospy.get_param("~ros_log_location")
        self.should_purge_log_on_startup_file = rospy.get_param("~should_purge_ros_log_on_startup_file")
        self.purge_log_on_startup = self.should_purge_log_on_startup()

        # clean log on startup if param is true
        if self.purge_log_on_startup:
            rospy.logwarn("Purging ROS log on startup !")
            print self.purge_log()

        self.purge_log_server = rospy.Service('/niryo_one/rpi/purge_ros_logs', SetInt,
                                              self.callback_purge_log)

        self.change_purge_log_on_startup_server = rospy.Service('/niryo_one/rpi/set_purge_ros_log_on_startup', SetInt,
                                                                self.callback_change_purge_log_on_startup)

        self.log_status_publisher = rospy.Publisher('/niryo_one/rpi/ros_log_status', LogStatus, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(3), self.publish_log_status)

        rospy.loginfo("Init Ros Log Manager OK") 

def __init__(self, args):
        if len(args)==1 or len(args)==2:
            self.max_speed = float(args[0])
            self.max_steering_angle = float(args[len(args)-1])
            cmd_topic = 'ackermann_cmd'
        elif len(args) == 3:
            self.max_speed = float(args[0])
            self.max_steering_angle = float(args[1])
            cmd_topic = '/' + args[2]
        else:
            self.max_speed = 0.2
            self.max_steering_angle = 0.7
            cmd_topic = 'ackermann_cmd'

        self.speed = 0
        self.steering_angle = 0
        self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
        self.drive_pub = rospy.Publisher(cmd_topic, AckermannDrive,
                                         queue_size=1)
        rospy.Timer(rospy.Duration(1.0/5.0), self.pub_callback, oneshot=False)
        rospy.loginfo('ackermann_drive_joyop_node initialized') 

def __init__(self, image_path=None):
        height = int(rospy.get_param("~grid_height", 800))
        width = int(rospy.get_param("~grid_width", 800))
        resolution = rospy.get_param("~grid_resolution", .25)
        ogrid_topic = rospy.get_param("/lqrrt_node/ogrid_topic", "/ogrid")

        self.grid_drawer = DrawGrid(height, width, image_path)
        self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)

        m = MapMetaData()
        m.resolution = resolution
        m.width = width
        m.height = height
        pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
        quat = np.array([0, 0, 0, 1])
        m.origin = Pose()
        m.origin.position.x, m.origin.position.y = pos[:2]
        self.map_meta_data = m

        rospy.Timer(rospy.Duration(1), self.pub_grid) 

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 run(self, UAV):
        if(self.called):
            rospy.Timer(self.duration, self.callbackTimer)
            self.called = True
        return self.isDone() 

def __init__(self):
        self.joy_enabled = False
        self.joint_mode = JointMode()

        joy_subscriber = rospy.Subscriber('joy', Joy, self.callback_joy)

        self.joystick_server = rospy.Service(
            '/niryo_one/joystick_interface/enable', SetInt, self.callback_enable_joystick)

        self.joystick_enabled_publisher = rospy.Publisher('/niryo_one/joystick_interface/is_enabled',
                                                          Bool, queue_size=1)

        rospy.Timer(rospy.Duration(2), self.publish_joystick_enabled) 

def __init__(self):
        self.sequence_autorun_status_file = rospy.get_param("~sequence_autorun_status_file")
        self.sequence_autorun_status_file = os.path.expanduser(self.sequence_autorun_status_file)
        self.lock = Lock()
        self.enabled, self.mode, self.sequence_ids = self.read_sequence_autorun_status()
        self.activated = False
        self.sequence_execution_index = -1
        self.flag_send_robot_to_home_position = False
        self.cancel_sequence = False
        self.is_sequence_running = False

        self.calibration_needed = None
        self.calibration_in_progress = None
        self.hardware_status_subscriber = rospy.Subscriber(
            '/niryo_one/hardware_status', HardwareStatus, self.sub_hardware_status)

        self.learning_mode_on = None
        self.learning_mode_subscriber = rospy.Subscriber(
            '/niryo_one/learning_mode', Bool, self.sub_learning_mode)

        # Wait for sequence action server to finish setup
        self.sequence_action_client = actionlib.SimpleActionClient('niryo_one/sequences/execute', SequenceAction)
        self.sequence_action_client.wait_for_server()

        self.sequence_autorun_status_publisher = rospy.Publisher(
            '/niryo_one/sequences/sequence_autorun_status', SequenceAutorunStatus, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(3.0), self.publish_sequence_autorun_status)

        self.set_sequence_autorun_server = rospy.Service(
            '/niryo_one/sequences/set_sequence_autorun', SetSequenceAutorun, self.callback_set_sequence_autorun)

        self.trigger_sequence_autorun = rospy.Service(
            '/niryo_one/sequences/trigger_sequence_autorun', SetInt, self.callback_trigger_sequence_autorun)

        self.sequence_autorun_thread = Thread(name="worker", target=self.execute_sequence_autorun_thread)
        self.sequence_autorun_thread.setDaemon(True)
        self.sequence_autorun_thread.start()

        rospy.loginfo('Init Sequence autorun OK') 

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 __init__(self, args):
        if len(args) == 1:
            max_speed = float(args[0])
            max_steering_angle = float(args[0])
        elif len(args) >= 2:
            max_speed = float(args[0])
            max_steering_angle = float(args[1])
        else:
            max_speed = 0.2
            max_steering_angle = 0.7

        if len(args) > 2:
            cmd_topic = '/' + args[2]
        else:
            cmd_topic = 'ackermann_cmd'

        self.speed_range = [-float(max_speed), float(max_speed)]
        self.steering_angle_range = [-float(max_steering_angle),
                                     float(max_steering_angle)]
        for key in key_bindings:
            key_bindings[key] = \
                    (key_bindings[key][0] * float(max_speed) / 5,
                     key_bindings[key][1] * float(max_steering_angle) / 5)

        self.speed = 0
        self.steering_angle = 0
        self.motors_pub = rospy.Publisher(
            cmd_topic, AckermannDriveStamped, queue_size=1)
        rospy.Timer(rospy.Duration(1.0/5.0), self.pub_callback, oneshot=False)
        self.print_state()
        self.key_loop() 

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_status_led(self, msg):
        self.respeaker.set_led_color(r=msg.r, g=msg.g, b=msg.b, a=msg.a)
        if self.timer_led and self.timer_led.is_alive():
            self.timer_led.shutdown()
        self.timer_led = rospy.Timer(rospy.Duration(3.0),
                                       lambda e: self.respeaker.set_led_trace(),
                                       oneshot=True) 

def __init__(self):
        # format of input audio data
        self.sample_rate = rospy.get_param("~sample_rate", 16000)
        self.sample_width = rospy.get_param("~sample_width", 2L)
        # language of STT service
        self.language = rospy.get_param("~language", "ja-JP")
        # ignore voice input while the robot is speaking
        self.self_cancellation = rospy.get_param("~self_cancellation", True)
        # time to assume as SPEAKING after tts service is finished
        self.tts_tolerance = rospy.Duration.from_sec(
            rospy.get_param("~tts_tolerance", 1.0))

        self.recognizer = SR.Recognizer()

        self.tts_action = None
        self.last_tts = None
        self.is_canceling = False
        if self.self_cancellation:
            self.tts_action = actionlib.SimpleActionClient(
                "sound_play", SoundRequestAction)
            if self.tts_action.wait_for_server(rospy.Duration(5.0)):
                self.tts_timer = rospy.Timer(rospy.Duration(0.1), self.tts_timer_cb)
            else:
                rospy.logerr("action '%s' is not initialized." % rospy.remap_name("sound_play"))
                self.tts_action = None

        self.pub_speech = rospy.Publisher(
            "speech_to_text", SpeechRecognitionCandidates, queue_size=1)
        self.sub_audio = rospy.Subscriber("audio", AudioData, self.audio_cb) 

def joint_state_cb(self, msg):
        with self.js_mutex:
            self.joint_states = msg

    ### @brief ROS Timer Callback function to stop the gripper moving past its limits when in PWM mode
    ### @param event [unused] - Timer event message 

def __init__(self):

        # Initialization parameters to control the Interbotix Arm
        rospy.wait_for_service("get_robot_info")
        srv_robot_info = rospy.ServiceProxy("get_robot_info", RobotInfo)                                                # ROS Service to get joint limit information among other things
        self.resp = srv_robot_info()                                                                                    # Store the robot information in this variable
        self.joint_indx_dict = dict(zip(self.resp.joint_names, range(self.resp.num_single_joints)))                     # Map each joint-name to their respective index in the joint_names list (which conveniently matches their index in the joint-limit lists)
        self.joy_msg = ArmJoyControl()                                                                                  # Incoming message coming from the 'joy_control' node
        self.arm_model = rospy.get_param("~robot_name")                                                                 # Arm-model type
        self.num_joints = self.resp.num_joints                                                                          # Number of joints in the arm
        self.speed_max = 3.0                                                                                            # Max scaling factor when bumping up joint speed
        self.gripper_pwm = 200                                                                                          # Initial gripper PWM value
        self.gripper_moving = False                                                                                     # Boolean that is set to 'True' if the gripper is moving - 'False' otherwise
        self.gripper_command = Float64()                                                                                # Float64 message to be sent to the 'gripper' joint
        self.gripper_index = self.num_joints + 1                                                                        # Index of the 'left_finger' joint in the JointState message
        self.follow_pose = True                                                                                         # True if going to 'Home' or 'Sleep' pose
        self.joint_states = None                                                                                        # Holds the most up-to-date JointState message
        self.js_mutex = threading.Lock()                                                                                # Mutex to make sure that 'self.joint_states' variable isn't being modified and read at the same time
        self.joint_commands = JointCommands()                                                                           # JointCommands message to command the arm joints as a group
        self.target_positions = list(self.resp.sleep_pos)                                                               # Holds the 'Sleep' or 'Home' joint values
        self.robot_des = getattr(mrd, self.arm_model)                                                                   # Modern Robotics robot description
        self.joy_speeds = {"course" : 2.0, "fine" : 2.0, "current" : 2.0}                                               # Dictionary containing the desired joint speed scaling factors
        self.pub_joint_commands = rospy.Publisher("joint/commands", JointCommands, queue_size=100, latch=True)          # ROS Publisher to command joint positions [rad]
        self.pub_gripper_command = rospy.Publisher("gripper/command", Float64, queue_size=100)                          # ROS Publisher to command gripper PWM values
        self.sub_joy_commands = rospy.Subscriber("joy/commands", ArmJoyControl, self.joy_control_cb)                    # ROS Subscriber to get the joystick commands
        self.sub_joint_states = rospy.Subscriber("joint_states", JointState, self.joint_state_cb)                       # ROS Subscriber to get the current joint states
        while (self.joint_states == None and not rospy.is_shutdown()): pass                                             # Wait until we know the current joint values of the robot
        self.joint_positions = list(self.joint_states.position[:self.num_joints])                                       # Holds the desired joint positions for the robot arm at any point in time
        self.yaw = 0.0                                                                                                  # Holds the desired 'yaw' of the end-effector w.r.t. the 'base_link' frame
        self.T_sy = np.identity(4)                                                                                      # Transformation matrix of a virtual frame with the same x, y, z, roll, and pitch values as 'base_link' but containing the 'yaw' of the end-effector
        self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.resp.sleep_pos)                           # Transformation matrix of the end-effector w.r.t. the 'base_link' frame
        self.T_yb = np.dot(mr.TransInv(self.T_sy), self.T_sb)                                                           # Transformation matrix of the end-effector w.r.t. T_sy
        tmr_controller = rospy.Timer(rospy.Duration(0.02), self.controller)                                             # ROS Timer to control the Interbotix Arm

    ### @brief Used to convert 'self.yaw' to a rotation matrix
    ### @param yaw - yaw angle to convert to a rotation matrix 

def StopRobotMotion(self):
        self.t1 = rospy.Timer(rospy.Duration(0.01),self._send_zero_command) 

def __init__(self, position_manager, trajectory_manager):
        self.trajectory_manager = trajectory_manager
        self.pos_manager = position_manager
        moveit_commander.roscpp_initialize(sys.argv)

        # Load all the sub-commanders
        self.move_group_arm = MoveGroupArm()
        self.arm_commander = ArmCommander(self.move_group_arm)
        self.tool_commander = ToolCommander()

        self.stop_trajectory_server = rospy.Service(
            'niryo_one/commander/stop_command', SetBool, self.callback_stop_command)

        self.reset_controller_pub = rospy.Publisher('/niryo_one/steppers_reset_controller',
                                                    Empty, queue_size=1)

        # robot action server
        self.server = actionlib.ActionServer('niryo_one/commander/robot_action',
                                             RobotMoveAction, self.on_goal, self.on_cancel, auto_start=False)
        self.current_goal_handle = None
        self.learning_mode_on = False
        self.joystick_enabled = False
        self.hardware_status = None
        self.is_active_server = rospy.Service(
            'niryo_one/commander/is_active', GetInt, self.callback_is_active)

        self.learning_mode_subscriber = rospy.Subscriber(
            '/niryo_one/learning_mode', Bool, self.callback_learning_mode)
        self.joystick_enabled_subscriber = rospy.Subscriber('/niryo_one/joystick_interface/is_enabled',
                                                            Bool, self.callback_joystick_enabled)
        self.hardware_status_subscriber = rospy.Subscriber(
            '/niryo_one/hardware_status', HardwareStatus, self.callback_hardware_status)

        self.validation = rospy.get_param("/niryo_one/robot_command_validation")
        self.parameters_validation = ParametersValidation(self.validation)

        # arm velocity
        self.max_velocity_scaling_factor = 100
        self.max_velocity_scaling_factor_pub = rospy.Publisher(
            '/niryo_one/max_velocity_scaling_factor', Int32, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_arm_max_velocity_scaling_factor)
        self.max_velocity_scaling_factor_server = rospy.Service(
            '/niryo_one/commander/set_max_velocity_scaling_factor', SetInt,
            self.callback_set_max_velocity_scaling_factor) 

def __init__(self):

        # Get params from rosparams
        self.timer_rate = rospy.get_param("~joystick_timer_rate_sec")
        self.validation = rospy.get_param("/niryo_one/robot_command_validation")
        self.joint_mode_timer = None

        self.synchronization_needed = True
        self.is_enabled = False

        self.axes = [0, 0, 0, 0, 0, 0, 0, 0]
        self.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.joint_states = JointState()
        self.joint_cmd = [0, 0, 0, 0, 0, 0]
        self.multiplier = DEFAULT_MULTIPLIER
        self.learning_mode_on = True
        self.current_tool_id = 0
        self.is_tool_active = False

        self.joint_state_subscriber = rospy.Subscriber('/joint_states',
                                                       JointState, self.callback_joint_states)

        self.learning_mode_subscriber = rospy.Subscriber(
            '/niryo_one/learning_mode', Bool, self.callback_learning_mode)

        self.current_tool_id_subscriber = rospy.Subscriber(
            "/niryo_one/current_tool_id", Int32, self.callback_current_tool_id)

        self.joint_trajectory_publisher = rospy.Publisher(
            '/niryo_one_follow_joint_trajectory_controller/command',
            JointTrajectory, queue_size=10)

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

        self.tool_action_client = actionlib.SimpleActionClient(
            'niryo_one/tool_action', ToolAction)
        self.tool_action_client.wait_for_server()

        self.joint_mode_timer = rospy.Timer(rospy.Duration(self.timer_rate), self.send_joint_trajectory)
        self.time_debounce_start_button = rospy.Time.now()
        self.time_debounce_A_button = rospy.Time.now()
        self.time_debounce_B_X_button = rospy.Time.now()  # common debounce for both buttons 

def __init__(self):
        rospy.loginfo("Starting wifi manager...")

        self.hotspot_ssid = rospy.get_param("~hotspot_ssid")
        # add robot unique identifier to ssid to make it unique and recognizable
        self.hotspot_ssid += " "
        self.hotspot_ssid += str(self.get_robot_unique_identifier())
        self.hotspot_password = rospy.get_param("~hotspot_password")
        self.filename_robot_name = rospy.get_param("~filename_robot_name")

        # Set filename for robot name
        set_filename_robot_name(self.filename_robot_name)

        # Get robot name
        self.robot_name = read_robot_name()
        if self.robot_name != '':
            self.hotspot_ssid = self.robot_name

        # Start set robot name service server
        self.set_robot_name_server = rospy.Service('/niryo_one/wifi/set_robot_name',
                                                   SetString, self.callback_set_robot_name)

        # Set Niryo One hotspot ssid and password
        set_hotspot_ssid(self.hotspot_ssid)
        set_hotspot_password(self.hotspot_password)

        # Start broadcast
        broadcast_thread = Thread(target=self.start_broadcast)
        broadcast_thread.setDaemon(True)
        broadcast_thread.start()

        # Check if connected to Wi-Fi. If not, start hotspot mode
        current_ssid = niryo_one_wifi.get_current_ssid()
        rospy.loginfo("Current ssid : " + str(current_ssid))
        if not niryo_one_wifi.is_connected_to_wifi():
            niryo_one_wifi.hard_enable_hotspot_with_ssid(self.hotspot_ssid, self.hotspot_password)
        else:
            rospy.loginfo("Already connected to a Wifi or in Hotspot mode")

        # Start Flask app
        flask_thread = Thread(target=self.run_flask_server)
        flask_thread.setDaemon(True)
        flask_thread.start()

        # Start wifi status publisher
        self.hotspot_state_publisher = rospy.Publisher('/niryo_one/wifi/hotspot', Bool, queue_size=2)
        rospy.Timer(rospy.Duration(1), self.send_hotspot_state)

        # Start hotspot subscriber (from button)
        self.activate_hotspot_server = rospy.Service('/niryo_one/wifi/set_hotspot',
                                                     SetInt, self.callback_activate_hotspot)

        rospy.loginfo("Wifi manager started") 

def __init__(self, odom_topic, ref_topic, move_topic, path_topic, tree_topic,
                 goal_topic, focus_topic, effort_topic, ogrid_topic, ogrid_threshold):
        """
        Initialize with topic names and ogrid threshold as applicable.
        Defaults are generated at the ROS params level.

        """
        # One-time initializations
        self.revisit_period = 0.05  # s
        self.ogrid = None
        self.ogrid_threshold = float(ogrid_threshold)
        self.state = None
        self.tracking = None
        self.done = True

        # Lil helpers
        self.rostime = lambda: rospy.Time.now().to_sec()
        self.intup = lambda arr: tuple(np.array(arr, dtype=np.int64))
        self.get_hood = lambda img, row, col: img[row-1:row+2, col-1:col+2]

        # Set-up planners
        self.behaviors_list = [car, boat, escape]
        for behavior in self.behaviors_list:
            behavior.planner.set_system(erf=self.erf)
            behavior.planner.set_runtime(sys_time=self.rostime)
            behavior.planner.constraints.set_feasibility_function(self.is_feasible)

        # Initialize resetable stuff
        self.reset()

        # Subscribers
        rospy.Subscriber(odom_topic, Odometry, self.odom_cb)
        rospy.Subscriber(ogrid_topic, OccupancyGrid, self.ogrid_cb)
        rospy.sleep(0.5)

        # Publishers
        self.ref_pub = rospy.Publisher(ref_topic, Odometry, queue_size=1)
        self.path_pub = rospy.Publisher(path_topic, PoseArray, queue_size=3)
        self.tree_pub = rospy.Publisher(tree_topic, PoseArray, queue_size=3)
        self.goal_pub = rospy.Publisher(goal_topic, PoseStamped, queue_size=3)
        self.focus_pub = rospy.Publisher(focus_topic, PointStamped, queue_size=3)
        self.eff_pub = rospy.Publisher(effort_topic, WrenchStamped, queue_size=3)
        self.sampspace_pub = rospy.Publisher(sampspace_topic, PolygonStamped, queue_size=3)
        self.guide_pub = rospy.Publisher(guide_topic, PointStamped, queue_size=3)

        # Actions
        self.move_server = actionlib.SimpleActionServer(move_topic, MoveAction, execute_cb=self.move_cb, auto_start=False)
        self.move_server.start()
        rospy.sleep(1)

        # Timers
        rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
        rospy.Timer(rospy.Duration(self.revisit_period), self.action_check) 

def __init__(self, robot_name, mrd=None, robot_model=None, moving_time=2.0, accel_time=0.3, gripper_pressure=0.5, use_time=True):
        rospy.init_node(robot_name + "_robot_manipulation")                                                                         # Initialize ROS Node
        rospy.wait_for_service(robot_name + "/get_robot_info")                                                                      # Wait for ROS Services to become available
        rospy.wait_for_service(robot_name + "/set_operating_modes")
        rospy.wait_for_service(robot_name + "/set_motor_register_values")
        srv_robot_info = rospy.ServiceProxy(robot_name + "/get_robot_info", RobotInfo)                                              # Create Service Proxies
        self.srv_set_op = rospy.ServiceProxy(robot_name + "/set_operating_modes", OperatingModes)
        self.srv_set_register = rospy.ServiceProxy(robot_name + "/set_motor_register_values", RegisterValues)
        self.resp = srv_robot_info()                                                                                                # Get Robot Info like joint limits
        self.use_time = use_time                                                                                                    # Determine if 'Drive Mode' is set to 'Time' or 'Velocity'
        self.set_trajectory_time(moving_time, accel_time)                                                                           # Change the Profile Velocity/Acceleration Registers in the Arm motors
        self.joint_indx_dict = dict(zip(self.resp.joint_names, range(self.resp.num_single_joints)))                                 # Map joint names to their respective indices in the joint limit lists
        self.joint_states = None                                                                                                    # Holds the current joint states of the arm
        self.js_mutex = threading.Lock()                                                                                            # Mutex to prevent writing/accessing the joint states variable at the same time
        self.pub_joint_commands = rospy.Publisher(robot_name + "/joint/commands", JointCommands, queue_size=100)                    # ROS Publisher to command the arm
        self.pub_single_command = rospy.Publisher(robot_name + "/single_joint/command", SingleCommand, queue_size=100)              # ROS Publisher to command a specified joint
        self.sub_joint_states = rospy.Subscriber(robot_name + "/joint_states", JointState, self.joint_state_cb)                     # ROS Subscriber to get the current joint states
        while (self.joint_states == None and not rospy.is_shutdown()): pass                                                         # Wait until the first JointState message is received
        if robot_model is None:
            robot_model = robot_name
        if (mrd is not None):
            self.robot_des = getattr(mrd, robot_model)                                                                              # Contains the Modern Robotics description of the desired arm model
            self.gripper_moving = False                                                                                             # When in PWM mode, False means the gripper PWM is 0; True means the gripper PWM in nonzero
            self.gripper_command = Float64()                                                                                        # ROS Message to hold the gripper PWM command
            self.set_gripper_pressure(gripper_pressure)                                                                             # Maps gripper pressure to a PWM range
            self.gripper_index = self.joint_states.name.index("left_finger")                                                        # Index of the 'left_finger' joint in the JointState message
            self.initial_guesses = [[0.0] * self.resp.num_joints for i in range(3)]                                                 # Guesses made up of joint values to seed the IK function
            self.initial_guesses[1][0] = np.deg2rad(-120)
            self.initial_guesses[2][0] = np.deg2rad(120)
            self.pub_gripper_command = rospy.Publisher(robot_name + "/gripper/command", Float64, queue_size=100)                    # ROS Publisher to command the gripper
            self.pub_joint_traj = rospy.Publisher(robot_name + "/arm_controller/joint_trajectory", JointTrajectory, queue_size=100) # ROS Pubilsher to command Cartesian trajectories
            self.waist_index = self.joint_states.name.index("waist")                                                                # Index of the 'waist' joint in the JointState 'name' list
            self.joint_positions = list(self.joint_states.position[self.waist_index:(self.resp.num_joints+self.waist_index)])       # Holds the desired joint positions
            self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_positions)                                  # Transformation matrix describing the pose of the end-effector w.r.t. the Space frame
            tmr_controller = rospy.Timer(rospy.Duration(0.02), self.controller)                                                     # ROS Timer to check gripper position
        if self.use_time:
            rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMoving Time: %.2f seconds\nAcceleration Time: %.2f seconds\nGripper Pressure: %d%%\nDrive Mode: Time-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100))
        else:
            rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMax Velocity: %d \nMax Acceleration: %d\nGripper Pressure: %d%%\nDrive Mode: Velocity-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100))
        rospy.sleep(1)                                                                                                              # Give time for the ROS Publishers to get set up

# ******************************** PRIVATE FUNCTIONS - DO NOT CALL THEM ********************************

    ### @brief ROS Subscriber Callback function to update the latest arm joint states
    ### @param msg - latest JointState message 

def joy_control_cb(self, msg):
        self.joy_msg = msg

        # check gripper_cmd
        if (self.joy_msg.gripper_cmd != 0):
            with self.js_mutex:
                gripper_pos = self.joint_states.position[self.gripper_index]
            if (self.joy_msg.gripper_cmd == ArmJoyControl.GRIPPER_OPEN and gripper_pos < self.resp.upper_gripper_limit):
                self.gripper_command.data = self.gripper_pwm
            elif (self.joy_msg.gripper_cmd == ArmJoyControl.GRIPPER_CLOSE and gripper_pos > self.resp.lower_gripper_limit):
                self.gripper_command.data = -self.gripper_pwm
            self.pub_gripper_command.publish(self.gripper_command)
            self.gripper_moving = True

        # check robot_pose
        if (self.joy_msg.robot_pose != 0):
            if (self.joy_msg.robot_pose == ArmJoyControl.HOME_POSE):
                self.target_positions = list(self.resp.home_pos)
            elif (self.joy_msg.robot_pose == ArmJoyControl.SLEEP_POSE):
                self.target_positions = list(self.resp.sleep_pos)
            self.follow_pose = True
            # reset all transforms
            self.yaw = 0.0
            self.T_sy[:2,:2] = self.yaw_to_rotation_matrix(self.yaw)
            self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.target_positions)
            self.T_yb = np.dot(mr.TransInv(self.T_sy), self.T_sb)

        # check speed_cmd
        if (self.joy_msg.speed_cmd != 0):
            if (self.joy_msg.speed_cmd == ArmJoyControl.SPEED_INC and self.joy_speeds["current"] < self.speed_max):
                self.joy_speeds["current"] += 0.25
            elif (self.joy_msg.speed_cmd == ArmJoyControl.SPEED_DEC and self.joy_speeds["current"] > 1):
                self.joy_speeds["current"] -= 0.25
            rospy.loginfo("Current scaling factor is %.2f." % self.joy_speeds["current"])

        # check toggle_speed_cmd
        if (self.joy_msg.toggle_speed_cmd != 0):
            if (self.joy_msg.toggle_speed_cmd == ArmJoyControl.SPEED_COURSE):
                self.joy_speeds["fine"] = self.joy_speeds["current"]
                self.joy_speeds["current"] = self.joy_speeds["course"]
                rospy.loginfo("Switched to Course Control")
            elif (self.joy_msg.toggle_speed_cmd == ArmJoyControl.SPEED_FINE):
                self.joy_speeds["course"] = self.joy_speeds["current"]
                self.joy_speeds["current"] = self.joy_speeds["fine"]
                rospy.loginfo("Switched to Fine Control")

        # check gripper_pwm_cmd
        if (self.joy_msg.gripper_pwm_cmd != 0):
            if (self.joy_msg.gripper_pwm_cmd == ArmJoyControl.GRIPPER_PWM_INC and self.gripper_pwm < 350):
                self.gripper_pwm += 25
            elif (self.joy_msg.gripper_pwm_cmd == ArmJoyControl.GRIPPER_PWM_DEC and self.gripper_pwm > 150):
                self.gripper_pwm -= 25
            rospy.loginfo("Current PWM command is %d." % self.gripper_pwm)

    ### @brief ROS Timer Callback function running at 50 Hz that updates joint positions constantly
    ### @param event - ROS timer message (unused)