Python std_msgs.msg.Int32() Examples

def listener():

	global obj
	global pub

	rospy.loginfo("Starting prediction node")

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

	rospy.Subscriber("sensor_read", Int32, read_sensor_data)

	pub = rospy.Publisher('predict', Int32, queue_size=1)


	obj = Classify_Data()

	obj.read_file('pos_readings.csv')

	obj.train()

	rospy.spin() 

def __init__(self, ros_command_interface):

        self.ros_command_interface = ros_command_interface

        self.server = actionlib.SimpleActionServer(
            'niryo_one/tool_action', ToolAction, self.tool_on_goal, False)

        self.change_tool_server = rospy.Service(
            'niryo_one/change_tool', SetInt, self.callback_change_tool)

        self.current_tool_id_publisher = rospy.Publisher(
            '/niryo_one/current_tool_id', Int32, queue_size=1)

        rospy.Timer(rospy.Duration(1 / 1.0), self.publish_current_tool_id)

        self.current_tool = None
        self.available_tools = None
        self.command_list = None
        self.create_tools() 

def listener():

	global obj
	global pub

	rospy.loginfo("Starting prediction node")

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

	rospy.Subscriber("sensor_read", Int32, read_sensor_data)

	pub = rospy.Publisher('predict', Int32, queue_size=1)


	obj = Classify_Data()

	obj.read_file('pos_readings.csv')

	obj.train()

	rospy.spin() 

def listener():
	print ' '
	print 'Begin monitor mode - listening to all fucntional topics'
	print '======================================================='
	print '         Use rqt_graph to check the connection         '
	print '======================================================='

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

	rospy.Subscriber("uarm_status",String, attchCallback)
	rospy.Subscriber("pump_control",UInt8, pumpCallack)
	rospy.Subscriber("pump_str_control",String, pumpStrCallack)

	rospy.Subscriber("read_coords",Int32, currentCoordsCallback)
	rospy.Subscriber("read_angles",Int32, readAnglesCallback)
	rospy.Subscriber("stopper_status",Int32, stopperStatusCallback)

	rospy.Subscriber("write_angles",Angles, writeAnglesCallback)
	rospy.Subscriber("move_to",Coords, moveToCallback)
	rospy.Subscriber("move_to_time",CoordsWithTime, moveToTimeCallback)
	rospy.Subscriber("move_to_time_s4",CoordsWithTS4, moveToTimeAndS4Callback)

	rospy.spin()
	pass


# show eroors 

def send_data():

    	rospy.init_node('virtual_sensor', anonymous=True)
	rospy.loginfo("Sending virtual sensor data")
	pub = rospy.Publisher('sensor_read', Int32, queue_size=1)
    	rate = rospy.Rate(10) # 10hz

    	while not rospy.is_shutdown():

        	sensor_reading = random.randint(0,30000)
        	pub.publish(sensor_reading)

        	rate.sleep() 

def execute():

	# define publisher and its topic 
	pub = rospy.Publisher('read_angles',Int32,queue_size = 10)
	rospy.init_node('report_angles_node',anonymous = True)
	rate = rospy.Rate(10)

	# report once		
	if len(sys.argv) == 1:
		pub.publish(1)
		time.sleep(0.15)
		print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))

	# report multiple time
	elif len(sys.argv) == 2:
		for i in range(0,int(sys.argv[1])):
			pub.publish(1)
			time.sleep(0.15)
			print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))

	elif len(sys.argv) == 3:
		for i in range(0,int(sys.argv[1])):
			pub.publish(1)
			time.sleep(0.15)
			print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4'))
			time.sleep(float(sys.argv[2])-0.15)
	
	else:
		raiseError()

	rate.sleep()

# main function 

def execute():

	# define publisher and its topic 
	pub = rospy.Publisher('read_coords',Int32,queue_size = 10)
	rospy.init_node('report_coords_node',anonymous = True)
	rate = rospy.Rate(10)
	
	# report once
	if len(sys.argv) == 1:
		pub.publish(1)
		time.sleep(0.1)
		print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z'))

	# report multiple time
	elif len(sys.argv) == 2:
		for i in range(0,int(sys.argv[1])):
			pub.publish(1)
			time.sleep(0.1)
			print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z'))

	elif len(sys.argv) == 3:
		for i in range(0,int(sys.argv[1])):
			pub.publish(1)
			time.sleep(0.1)
			print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z'))
			time.sleep(float(sys.argv[2])-0.1)
	else:
		raiseError()

	rate.sleep()
			
# main function 

def __init__(self):
    print('constructor')
    # Init QWidget
    super(CalibWidget, self).__init__()
    self.setObjectName('CalibWidget')

    # load UI
    ui_file = os.path.join(rospkg.RosPack().get_path('dvs_calibration_gui'), 'resource', 'widget.ui')
    loadUi(ui_file, self)

    # init and start update timer for data, the timer calls the function update_info all 40ms    
    self._update_info_timer = QTimer(self)
    self._update_info_timer.timeout.connect(self.update_info)
    self._update_info_timer.start(40)

    self.button_reset.pressed.connect(self.on_button_reset_pressed)
    self.button_start.pressed.connect(self.on_button_start_calibration_pressed)
    self.button_save.pressed.connect(self.on_button_save_calibration_pressed)
    
    self._sub_pattern_detections = rospy.Subscriber('dvs_calibration/pattern_detections', Int32, self.pattern_detections_cb)    
    self._sub_calibration_output = rospy.Subscriber('dvs_calibration/output', String, self.calibration_output_cb)

    print('reset')

    self.on_button_reset_pressed()
    
    print('reset done') 

def __init__(self):

		# The OpenCog API. This is used to send face data to OpenCog.
		self.atomo = AtomicMsgs()
		self.atomo.create_face_octomap()

		# List of currently visible faces
		self.visible_faces = []

		# Subscribed pi_vision topics and events
		self.TOPIC_FACE_EVENT = "/camera/face_event"
		self.EVENT_NEW_FACE = "new_face"
		self.EVENT_LOST_FACE = "lost_face"
		self.EVENT_RECOGNIZED_FACE = "recognized_face"
		# Overrides current face being tracked by WebUI
		self.EVENT_TRACK_FACE = "track_face"

		self.TOPIC_FACE_LOCATIONS = "/camera/face_locations"

		# Face appearance/disappearance from pi_vision
		rospy.Subscriber(self.TOPIC_FACE_EVENT, FaceEvent, self.face_event_cb)

		# Face location information from pi_vision
		rospy.Subscriber(self.TOPIC_FACE_LOCATIONS, Faces, self.face_loc_cb)

		rospy.Subscriber("/behavior_control", Int32, self.behavior_control_cb)

		# Control Eyes and face by default
		self.control_mode = 255

	# ----------------------------------------------------------
	# Start tracking a face 

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 send_data():

    	rospy.init_node('virtual_sensor', anonymous=True)
	rospy.loginfo("Sending virtual sensor data")
	pub = rospy.Publisher('sensor_read', Int32, queue_size=1)
    	rate = rospy.Rate(10) # 10hz

    	while not rospy.is_shutdown():

        	sensor_reading = random.randint(0,30000)
        	pub.publish(sensor_reading)

        	rate.sleep() 

def callback(msg):
    doubled = Int32()
    doubled.data = msg.data * 2

    pub.publish(doubled) 

def confirm(self, name, id):
        """
        Confirms the state machine and triggers the creation of the structural message.
        It is mandatory to call this function at the top-level state machine
        between building it and starting its execution.
        
        @type name: string
        @param name: The name of this state machine to identify it.
        """
        self.name = name
        self.id = id

        self._pub.createPublisher('flexbe/mirror/sync', BehaviorSync, _latch = True)   # Update mirror with currently active state (high bandwidth mode)
        self._pub.createPublisher('flexbe/mirror/preempt', Empty, _latch = True)       # Preempts the mirror
        self._pub.createPublisher('flexbe/mirror/structure', ContainerStructure)       # Sends the current structure to the mirror
        self._pub.createPublisher('flexbe/log', BehaviorLog)                           # Topic for logs to the GUI
        self._pub.createPublisher('flexbe/command_feedback', CommandFeedback)          # Gives feedback about executed commands to the GUI

        self._sub.subscribe('flexbe/command/autonomy', UInt8, self._set_autonomy_level)
        self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback)
        self._sub.subscribe('flexbe/command/attach', UInt8, self._attach_callback)
        self._sub.subscribe('flexbe/request_mirror_structure', Int32, self._mirror_structure_callback)

        StateLogger.initialize(name)
        StateLogger.log('flexbe.initialize', None, behavior=name, autonomy=OperatableStateMachine.autonomy_level)
        if OperatableStateMachine.autonomy_level != 255:
            self._enable_ros_control()

        rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
        self._notify_start() 

def _start_mirror(self, msg):
        with self._sync_lock:
            rate = rospy.Rate(10)
            while self._stopping:
                rate.sleep()

            if self._running:
                rospy.logwarn('Tried to start mirror while it is already running, will ignore.')
                return

            if len(msg.args) > 0:
                self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror"

            self._active_id = msg.behavior_id

            while self._sm is None and len(self._struct_buffer) > 0:
                struct = self._struct_buffer[0]
                self._struct_buffer = self._struct_buffer[1:]
                if struct.behavior_id == self._active_id:
                    self._mirror_state_machine(struct)
                    rospy.loginfo('Mirror built.')
                else:
                    rospy.logwarn('Discarded mismatching buffered structure for checksum %s' % str(struct.behavior_id))

            if self._sm is None:
                rospy.logwarn('Missing correct mirror structure, requesting...')
                rospy.sleep(0.2) # no clean wayacquire to wait for publisher to be ready...
                self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id))
                self._active_id = msg.behavior_id
                return

        self._execute_mirror() 

def __init__(self):
        '''
        Constructor
        '''
        self._sm = None

        smach.set_loggers (
            rospy.logdebug, # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr
        )
        
        # set up proxys for sm <--> GUI communication
        # publish topics
        self._pub = ProxyPublisher({'flexbe/behavior_update': String,
                                    'flexbe/request_mirror_structure': Int32})
            
        self._running = False
        self._stopping = False
        self._active_id = 0
        self._starting_path = None
        self._current_struct = None
        self._sync_lock = threading.Lock()

        self._outcome_topic = 'flexbe/mirror/outcome'

        self._struct_buffer = list()
        
        # listen for mirror message
        self._sub = ProxySubscriberCached()
        self._sub.subscribe(self._outcome_topic, UInt8)
        self._sub.enable_buffer(self._outcome_topic)

        self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback)
        self._sub.subscribe('flexbe/status', BEStatus, self._status_callback)
        self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback)
        self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback) 

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 publish_current_tool_id(self, event):
        msg = Int32()
        if self.current_tool:
            msg.data = self.current_tool.id
        else:
            msg.data = 0
        self.current_tool_id_publisher.publish(msg) 

def break_point(self):
            msg = Int32()
            msg.data = 1
            self.break_point_publisher.publish(msg)
            # This delay makes sure the program has time
            # to stop, before executing next command
            rospy.sleep(0.5) 

def __init__(self, sequence_manager):
        self.server = actionlib.ActionServer('niryo_one/sequences/execute',
                                             SequenceAction, self.on_goal, self.on_cancel, auto_start=False)

        self.seq_code_executor = SequenceCodeExecutor()

        self.current_goal_handle = None

        self.seq_manager = sequence_manager

        self.break_point_subscriber = rospy.Subscriber("/niryo_one/blockly/break_point", Int32,
                                                       self.callback_break_point) 

def publish_arm_max_velocity_scaling_factor(self, event):
        msg = Int32()
        msg.data = self.max_velocity_scaling_factor
        self.max_velocity_scaling_factor_pub.publish(msg) 

def start_ros_subscribers(self):
        self.joint_state_sub = rospy.Subscriber('/joint_states', JointState, self.sub_joint_states)
        self.robot_state_sub = rospy.Subscriber('/niryo_one/robot_state', RobotState, self.sub_robot_state)
        self.selected_tool_id_sub = rospy.Subscriber('/niryo_one/current_tool_id', Int32, self.sub_selected_tool_id)
        self.learning_mode_sub = rospy.Subscriber('/niryo_one/learning_mode', Bool, self.sub_learning_mode)
        self.joystick_enabled_sub = rospy.Subscriber('/niryo_one/joystick_interface/is_enabled', Bool,
                                                     self.sub_joystick_enabled)
        self.hardware_status_sub = rospy.Subscriber('/niryo_one/hardware_status', HardwareStatus,
                                                    self.sub_hardware_status)
        self.ros_log_status_sub = rospy.Subscriber('/niryo_one/rpi/ros_log_status', LogStatus, self.sub_ros_log_status)
        self.software_version_sub = rospy.Subscriber('/niryo_one/software_version', SoftwareVersion,
                                                     self.sub_software_version)

        rospy.Subscriber('/niryo_one/kits/conveyor_1_feedback', ConveyorFeedback,
                         self.sub_conveyor_1_feedback)
        rospy.Subscriber('/niryo_one/kits/conveyor_2_feedback', ConveyorFeedback,
                         self.sub_conveyor_2_feedback) 

def __init__(self):
            self.service_timeout = rospy.get_param("/niryo_one/python_api/service_timeout")
            self.action_connection_timeout = rospy.get_param("/niryo_one/python_api/action_connection_timeout")
            self.action_execute_timeout = rospy.get_param("/niryo_one/python_api/action_execute_timeout")
            self.action_preempt_timeout = rospy.get_param("/niryo_one/python_api/action_preempt_timeout")
            self.tool_command_list = rospy.get_param("/niryo_one_tools/command_list")

            # Subscribers
            self.joint_states_sub = rospy.Subscriber('/joint_states', JointState, self.sub_joint_states)
            self.robot_state_sub = rospy.Subscriber('/niryo_one/robot_state', RobotState, self.sub_robot_state)
            self.hardware_status_sub = rospy.Subscriber('/niryo_one/hardware_status', HardwareStatus,
                                                        self.sub_hardware_status)
            self.learning_mode_sub = rospy.Subscriber('/niryo_one/learning_mode', Bool, self.sub_learning_mode)
            self.digital_io_state_sub = rospy.Subscriber('/niryo_one/rpi/digital_io_state', DigitalIOState,
                                                         self.sub_digital_io_state)
            self.tool_id_sub = rospy.Subscriber('/niryo_one/current_tool_id', Int32,
                                                self.sub_current_tool_id)
            self.conveyor_1_feedback_sub = rospy.Subscriber('/niryo_one/kits/conveyor_1_feedback', ConveyorFeedback,
                                                            self.sub_conveyor_1_feedback)
            self.conveyor_2_feedback_sub = rospy.Subscriber('/niryo_one/kits/conveyor_2_feedback', ConveyorFeedback,
                                                            self.sub_conveyor_2_feedback)

            # Highlight publisher (to highlight blocks in Blockly interface)
            self.highlight_block_publisher = rospy.Publisher('/niryo_one/blockly/highlight_block', String,
                                                             queue_size=10)

            # Break point publisher (for break point blocks in Blockly interface)
            self.break_point_publisher = rospy.Publisher('/niryo_one/blockly/break_point', Int32, queue_size=10)

            self.video_stream_subscriber = rospy.Subscriber('/niryo_one_vision/compressed_video_stream',
                                                            CompressedImage, self.sub_stream_video,
                                                            queue_size=1)

            self.joints = None
            self.pose = None
            self.hw_status = None
            self.learning_mode_on = None
            self.digital_io_state = None
            self.current_tool_id = None
            self.compressed_image_message = None

            self.conveyor_1_feedback = None
            self.conveyor_2_feedback = None

            rospy.sleep(0.1)

        #
        # Subscribers callbacks
        # 

def on_timer(self, event):
        stamp = event.current_real or rospy.Time.now()
        is_voice = self.respeaker.is_voice()
        doa_rad = math.radians(self.respeaker.direction - 180.0)
        doa_rad = angles.shortest_angular_distance(
            doa_rad, math.radians(self.doa_yaw_offset))
        doa = math.degrees(doa_rad)

        # vad
        if is_voice != self.prev_is_voice:
            self.pub_vad.publish(Bool(data=is_voice))
            self.prev_is_voice = is_voice

        # doa
        if doa != self.prev_doa:
            self.pub_doa_raw.publish(Int32(data=doa))
            self.prev_doa = doa

            msg = PoseStamped()
            msg.header.frame_id = self.sensor_frame_id
            msg.header.stamp = stamp
            ori = T.quaternion_from_euler(math.radians(doa), 0, 0)
            msg.pose.position.x = self.doa_xy_offset * np.cos(doa_rad)
            msg.pose.position.y = self.doa_xy_offset * np.sin(doa_rad)
            msg.pose.orientation.w = ori[0]
            msg.pose.orientation.x = ori[1]
            msg.pose.orientation.y = ori[2]
            msg.pose.orientation.z = ori[3]
            self.pub_doa.publish(msg)

        # speech audio
        if is_voice:
            self.speech_stopped = stamp
        if stamp - self.speech_stopped < rospy.Duration(self.speech_continuation):
            self.is_speeching = True
        elif self.is_speeching:
            buf = self.speech_audio_buffer
            self.speech_audio_buffer = str()
            self.is_speeching = False
            duration = 8.0 * len(buf) * self.respeaker_audio.bitwidth
            duration = duration / self.respeaker_audio.rate / self.respeaker_audio.bitdepth
            rospy.loginfo("Speech detected for %.3f seconds" % duration)
            if self.speech_min_duration <= duration < self.speech_max_duration:

                self.pub_speech_audio.publish(AudioData(data=buf)) 

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 main_fcn():
	pub = rospy.Publisher('joint_states',JointState,queue_size = 10)
	pub2 = rospy.Publisher('read_angles',Int32,queue_size = 10)
	pub3 = rospy.Publisher('uarm_status',String,queue_size = 100)

	rospy.init_node('display',anonymous = True)
	rate = rospy.Rate(20)
	

	joint_state_send = JointState()
	joint_state_send.header = Header()
	joint_state_send.name = ['base_to_base_rot','base_rot_to_link_1','link_1_to_link_2' ,'link_2_to_link_3','link_3_to_link_end']


	joint_state_send.name = joint_state_send.name+ ['link_1_to_add_1','link_2_to_add_4','link_3_to_add_2','base_rot_to_add_3','link_2_to_add_5']
	
	angle = {}

	trigger = 1

	while not rospy.is_shutdown():
		joint_state_send.header.stamp = rospy.Time.now()

		pub2.publish(1)
		if trigger == 1:
			pub3.publish("detach")
			time.sleep(1)
			trigger = 0		
				
		angle['s1'] = rospy.get_param('servo_1')*math.pi/180
		angle['s2'] = rospy.get_param('servo_2')*math.pi/180
		angle['s3'] = rospy.get_param('servo_3')*math.pi/180
		angle['s4'] = rospy.get_param('servo_4')*math.pi/180

		joint_state_send.position = [angle['s1'],angle['s2'],-angle['s2']-angle['s3'],angle['s3'],angle['s4']]
		joint_state_send.position = joint_state_send.position + [-angle['s2']-angle['s3'],angle['s3'],PI/2-angle['s3']]
		joint_state_send.position = joint_state_send.position + [angle['s2']-PI,angle['s2']+angle['s3']-PI/2]
		joint_state_send.velocity = [0]
		joint_state_send.effort = []
		
		pub.publish(joint_state_send)
		rate.sleep() 

def execute():

	# define publisher and its topic 
	pub = rospy.Publisher('stopper_status',Int32,queue_size = 10)
	rospy.init_node('report_stopper_node',anonymous = True)
	rate = rospy.Rate(10)
	
	# report once
	if len(sys.argv) == 1:

		pub.publish(1)
		time.sleep(0.1)
		status = rospy.get_param('stopper_status')

		if status == 'HIGH':
			print 'Stopper is actived'
		elif status == 'LOW':
			print 'Stopper is not actived'

	# report multiple time
	elif len(sys.argv) == 2:
		for i in range(0,int(sys.argv[1])):
			pub.publish(1)
			time.sleep(0.1)

			status = rospy.get_param('stopper_status')
			if status == 'HIGH':
				print 'Stopper is actived'
			elif status == 'LOW':
				print 'Stopper is not actived'

	elif len(sys.argv) == 3:
		for i in range(0,int(sys.argv[1])):

			pub.publish(1)
			time.sleep(0.1)

			status = rospy.get_param('stopper_status')
			if status == 'HIGH':
				print 'Stopper is actived'
			elif status == 'LOW':
				print 'Stopper is not actived'
			time.sleep(float(sys.argv[2])-0.1)
	else: 
		raiseError()

	rate.sleep()

# main function 

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)