Python rospy.Timer方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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') 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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...") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
def run(self, UAV):
        if(self.called):
            rospy.Timer(self.duration, self.callbackTimer)
            self.called = True
        return self.isDone() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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') 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import Timer [as 别名]
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)