Python std_msgs.msg.Empty() Examples

def reset_controller(self):
        msg = Empty()
        self.reset_controller_pub.publish(msg)
        # very important delay to avoid unexpected issues from ros_control
        rospy.sleep(0.05) 

def _initSubscribers(self):
        """
        INTERNAL METHOD, initializes the ROS subscribers
        """
        rospy.Subscriber(
            '/joint_angles',
            JointAnglesWithSpeed,
            self._jointAnglesCallback)

        rospy.Subscriber(
            '/cmd_vel',
            Twist,
            self._velocityCallback)

        rospy.Subscriber(
            '/move_base_simple/goal',
            MovetoPose,
            self._moveToCallback)

        rospy.Subscriber(
            '/move_base_simple/cancel',
            Empty,
            self._killMoveCallback) 

def __init__(self):
		self._behavior_started = False
		self._preempt_requested = False
		self._current_state = None
		self._active_behavior_id = None

		self._pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=100)
		self._preempt_pub = rospy.Publisher('flexbe/command/preempt', Empty, queue_size=100)
		self._status_sub = rospy.Subscriber('flexbe/status', BEStatus, self._status_cb)
		self._state_sub = rospy.Subscriber('flexbe/behavior_update', String, self._state_cb)

		self._as = actionlib.SimpleActionServer('flexbe/execute_behavior', BehaviorExecutionAction, None, False)
		self._as.register_preempt_callback(self._preempt_cb)
		self._as.register_goal_callback(self._goal_cb)

		self._rp = RosPack()
		self._behavior_lib = BehaviorLibrary()

		# start action server after all member variables have been initialized
		self._as.start()

		rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) 

def __init__(self,
                 json_model_path,
                 weights_path, target_size=(200, 200),
                 crop_size=(150, 150),
                 imgs_rootpath="../models"):

        self.pub = rospy.Publisher("cnn_predictions", CNN_out, queue_size=5)
        self.feedthrough_sub = rospy.Subscriber("state_change", Bool, self.callback_feedthrough, queue_size=1)
        self.land_sub = rospy.Subscriber("land", Empty, self.callback_land, queue_size=1)

        self.use_network_out = False
        self.imgs_rootpath = imgs_rootpath

        # Set keras utils
        K.set_learning_phase(TEST_PHASE)

        # Load json and create model
        model = utils.jsonToModel(json_model_path)
        # Load weights
        model.load_weights(weights_path)
        print("Loaded model from {}".format(weights_path))

        model.compile(loss='mse', optimizer='sgd')
        self.model = model
        self.target_size = target_size
        self.crop_size = crop_size 

def _heartbeat_worker(self):
        while True:
            self._pub.publish('flexbe/heartbeat', Empty())
            time.sleep(1) 

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 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 _enable_ros_control(self):
        super(EventState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._repeat_topic, Empty)
        self._sub.subscribe(self._pause_topic, Bool) 

def _enable_ros_control(self):
        super(PreemptableState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._preempt_topic, Empty)
        PreemptableState.preempt = False 

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 on_enter(self, userdata):
        self._pub.publish(self._topic, Empty()) 

def __init__(self, topic):
        '''
		Constructor
		'''
        super(PublisherEmptyState, self).__init__(outcomes=['done'])

        self._topic = topic
        self._pub = ProxyPublisher({self._topic: Empty}) 

def __init__(self, reconfig_server, limb = "right"):
        self._dyn = reconfig_server

        # control parameters
        self._rate = 1000.0  # Hz
        self._missed_cmds = 20.0  # Missed cycles before triggering timeout

        # create our limb instance
        self._limb = intera_interface.Limb(limb)

        # initialize parameters
        self._springs = dict()
        self._damping = dict()
        self._start_angles = dict()

        # create cuff disable publisher
        cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)

        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Running. Ctrl-c to quit") 

def __init__(self):
        # Define parameters
        self.joy_state_prev = GamepadState()
        # if None then not in agent mode, otherwise contains time of latest enable/ping
        self.agent_mode_t = None
        self.flip_dir = 0

        # Start ROS node
        rospy.init_node('gamepad_marshall_node')

        # Load parameters
        self.agent_mode_timeout_sec = rospy.get_param(
            '~agent_mode_timeout_sec', 1.0)

        self.pub_takeoff = rospy.Publisher(
            'takeoff', Empty,  queue_size=1, latch=False)
        self.pub_throw_takeoff = rospy.Publisher(
            'throw_takeoff', Empty,  queue_size=1, latch=False)
        self.pub_land = rospy.Publisher(
            'land', Empty,  queue_size=1, latch=False)
        self.pub_palm_land = rospy.Publisher(
            'palm_land', Empty,  queue_size=1, latch=False)
        self.pub_reset = rospy.Publisher(
            'reset', Empty,  queue_size=1, latch=False)
        self.pub_flattrim = rospy.Publisher(
            'flattrim', Empty,  queue_size=1, latch=False)
        self.pub_flip = rospy.Publisher(
            'flip', UInt8,  queue_size=1, latch=False)
        self.pub_cmd_out = rospy.Publisher(
            'cmd_vel', Twist, queue_size=10, latch=False)
        self.pub_fast_mode = rospy.Publisher(
            'fast_mode', Bool,  queue_size=1, latch=False)
        self.sub_agent_cmd_in = rospy.Subscriber(
            'agent_cmd_vel_in', Twist, self.agent_cmd_cb)
        self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb)
        rospy.loginfo('Gamepad marshall node initialized') 

def __init__(self, configs, wait_time=3):
        """
        The constructor for LoCoBotGripper class.

        :param configs: configurations for gripper
        :param wait_time: waiting time for opening/closing gripper
        :type configs: YACS CfgNode
        :type wait_time: float
        """
        super(LoCoBotGripper, self).__init__(configs=configs)
        self._gripper_state_lock = threading.RLock()
        self._gripper_state = None
        # Publishers and subscribers
        self.wait_time = wait_time
        self.pub_gripper_close = rospy.Publisher(
            self.configs.GRIPPER.ROSTOPIC_GRIPPER_CLOSE, Empty, queue_size=1
        )
        self.pub_gripper_open = rospy.Publisher(
            self.configs.GRIPPER.ROSTOPIC_GRIPPER_OPEN, Empty, queue_size=1
        )
        rospy.Subscriber(
            self.configs.GRIPPER.ROSTOPIC_GRIPPER_STATE,
            Int8,
            self._callback_gripper_state,
        ) 

def __init__(self, base):
        self.should_stop = False
        self.bumper = False
        self.cliff = False
        self.wheel_drop = False
        self.subscribers = []

        if base == "create":
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_BUMPER, Bumper, self.bumper_callback_create
            )
            self.subscribers.append(s)
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_CLIFF, Empty, self.cliff_callback
            )
            self.subscribers.append(s)
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_WHEELDROP, Empty, self.wheeldrop_callback
            )
            self.subscribers.append(s)
        else:
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_BUMPER,
                BumperEvent,
                self.bumper_callback_kobuki,
            )
            self.subscribers.append(s)
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_CLIFF, CliffEvent, self.cliff_callback
            )
            self.subscribers.append(s)
            s = rospy.Subscriber(
                self.configs.BASE.ROSTOPIC_WHEELDROP,
                WheelDropEvent,
                self.wheeldrop_callback,
            )
            self.subscribers.append(s) 

def __init__(self):

        # Publishers and subscribers
        self.pub_gripper_close = rospy.Publisher(
            ROSTOPIC_GRIPPER_CLOSE, Empty, queue_size=1
        )
        self.pub_gripper_open = rospy.Publisher(
            ROSTOPIC_GRIPPER_OPEN, Empty, queue_size=1
        ) 

def __trigger_update(self):
        # Let ROS handle the threading for me.
        self.update_pub.publish(Empty()) 

def get_calibration_camera(self):
        service_name = self.__services_name['get_calibration_camera_service']
        rospy.wait_for_service(service_name)

        try:
            calibra_service = rospy.ServiceProxy(service_name, GetCalibrationCam)
            res = calibra_service(Empty())
            return res
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e 

def _killMoveCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        '/move_base_simple/cancel' topic. This callback is used to stop the
        robot's base from moving

        Parameters:
            msg - an empty ROS message, with the Empty type
        """
        self.robot.moveTo(0, 0, 0, _async=True) 

def __init__(self):
        # Initialize ROS
        rospy.init_node('flock_driver_node', anonymous=False)

        # ROS publishers
        self._flight_data_pub = rospy.Publisher('flight_data', FlightData, queue_size=10)
        self._image_pub = rospy.Publisher('image_raw', Image, queue_size=10)

        # ROS subscriptions
        rospy.Subscriber('cmd_vel', Twist, self.cmd_vel_callback)
        rospy.Subscriber('takeoff', Empty, self.takeoff_callback)
        rospy.Subscriber('land', Empty, self.land_callback)
        rospy.Subscriber('flip', Flip, self.flip_callback)

        # ROS OpenCV bridge
        self._cv_bridge = CvBridge()

        # Connect to the drone
        self._drone = tellopy.Tello()
        self._drone.connect()
        self._drone.wait_for_connection(60.0)
        rospy.loginfo('connected to drone')

        # Listen to flight data messages
        self._drone.subscribe(self._drone.EVENT_FLIGHT_DATA, self.flight_data_callback)

        # Start video thread
        self._stop_request = threading.Event()
        video_thread = threading.Thread(target=self.video_worker)
        video_thread.start()

        # Spin until interrupted
        rospy.spin()

        # Force a landing
        self._drone.land()

        # Stop the video thread
        self._stop_request.set()
        video_thread.join(timeout=2)

        # Shut down the drone
        self._drone.quit()
        self._drone = None 

def __init__(
        self,
        configs,
        control_mode="position",
        moveit_planner="RRTConnectkConfigDefault",
        use_moveit=True,
    ):
        """
        The constructor for LoCoBotArm class.

        :param configs: configurations read from config file
        :param control_mode: Choose between 'position',
                            'velocity' and 'torque' control
        :param moveit_planner: Planner name for moveit,
                               only used if planning_mode = 'moveit'.
        :param use_moveit: use moveit or not, default is True
        :type configs: YACS CfgNode
        :type control_mode: string
        :type moveit_planner: string
        :type use_moveit: bool
        """
        use_arm = rospy.get_param("use_arm", False)
        use_sim = rospy.get_param("use_sim", False)
        use_arm = use_arm or use_sim
        if not use_arm:
            rospy.logwarn(
                "Neither use_arm, nor use_sim, is not set to "
                "True when the LoCoBot driver is launched."
                "You may not be able to command the "
                "arm correctly using PyRobot!!!"
            )
            return
        self.CONTROL_MODES = {"position": 0, "velocity": 1, "torque": 2}
        self.mode_control = self.CONTROL_MODES[control_mode]
        super(LoCoBotArm, self).__init__(
            configs=configs,
            moveit_planner=moveit_planner,
            analytical_ik=AIK,
            use_moveit=use_moveit,
        )

        self.joint_stop_pub = rospy.Publisher(
            self.configs.ARM.ROSTOPIC_STOP_EXECUTION, Empty, queue_size=1
        )
        # Services
        if self.mode_control == self.CONTROL_MODES["position"]:
            self.joint_cmd_srv = rospy.ServiceProxy(
                self.configs.ARM.ROSSERVICE_JOINT_COMMAND, JointCommand
            )
        elif self.mode_control == self.CONTROL_MODES["torque"]:
            self.torque_cmd_srv = rospy.ServiceProxy(
                self.configs.ARM.ROSTOPIC_TORQUE_COMMAND, JointCommand
            ) 

def __init__(self):
        entropy_node_name = '/grasp_entropy_node'
        rospy.wait_for_service(entropy_node_name + '/update_grid')
        self.entropy_srv = rospy.ServiceProxy(entropy_node_name + '/update_grid', NextViewpoint)
        rospy.wait_for_service(entropy_node_name + '/reset_grid')
        self.entropy_reset_srv = rospy.ServiceProxy(entropy_node_name + '/reset_grid', EmptySrv)
        rospy.wait_for_service(entropy_node_name + '/add_failure_point')
        self.add_failure_point_srv = rospy.ServiceProxy(entropy_node_name + '/add_failure_point', AddFailurePoint)

        self.curr_velocity_publish_rate = 100.0  # Hz
        self.curr_velo_pub = rospy.Publisher('/cartesian_velocity_node_controller/cartesian_velocity', Twist, queue_size=1)
        self.max_velo = 0.1
        self.curr_velo = Twist()
        self.best_grasp = Pose()
        self.viewpoints = 0
        self.grasp_width = 0.10
        self._in_velo_loop = False

        self.update_rate = 10.0  # Hz
        update_topic_name = '~/update'
        self.update_pub = rospy.Publisher(update_topic_name, Empty, queue_size=1)
        rospy.Subscriber(update_topic_name, Empty, self.__update_callback, queue_size=1)

        self.cs = ControlSwitcher({'moveit': 'position_joint_trajectory_controller',
                                   'velocity': 'cartesian_velocity_node_controller'})
        self.cs.switch_controller('moveit')
        self.pc = PandaCommander(group_name='panda_arm_hand')

        self.robot_state = None
        self.ROBOT_ERROR_DETECTED = False
        self.BAD_UPDATE = False
        rospy.Subscriber('/franka_state_controller/franka_states', FrankaState, self.__robot_state_callback, queue_size=1)

        # Centre and above the bin
        self.pregrasp_pose = [(rospy.get_param('/grasp_entropy_node/histogram/bounds/x2') + rospy.get_param('/grasp_entropy_node/histogram/bounds/x1'))/2,
                              (rospy.get_param('/grasp_entropy_node/histogram/bounds/y2') + rospy.get_param('/grasp_entropy_node/histogram/bounds/y1'))/2 + 0.08,
                              rospy.get_param('/grasp_entropy_node/height/z1') + 0.066,
                              2**0.5/2, -2**0.5/2, 0, 0]

        self.last_weight = 0
        self.__weight_increase_check()

        self.experiment = Experiment() 

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)