我们从Python开源项目中，提取了以下28个代码示例，用于说明如何使用std_msgs.msg.Empty()。
def save_to_memory(self, perception_name, data={}): if data == {}: rospy.logwarn('Empty data inserted...') return for item in data.keys(): if not item in self.conf_data[perception_name]['data'].keys(): rospy.logwarn('[%s -- wrong data inserted [%s]...'%(perception_name, item)) return srv_req = WriteDataRequest() srv_req.perception_name = perception_name srv_req.data = json.dumps(data) srv_req.by = rospy.get_name() target_memory = self.conf_data[perception_name]['target_memory'] try: rospy.wait_for_service('/%s/write_data'%target_memory) self.dict_srv_wr[target_memory](srv_req) except rospy.ServiceException as e: pass
def read_from_memory(self, target_memory, data): if data == {}: rospy.logwarn('Empty data requested...') return req = ReadDataRequest() req.perception_name = data['perception_name'] req.query = data['query'] for item in data['data']: req.data.append(item) resonse = self.dict_srv_rd[target_memory](req) if not response.result: return {} results = json.loads(response.data) return results
def __init__(self): State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints']) # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc. self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1) # Start thread to listen for reset messages to clear the waypoint queue def wait_for_path_reset(): """thread worker function""" global waypoints while not rospy.is_shutdown(): data = rospy.wait_for_message('/path_reset', Empty) rospy.loginfo('Recieved path RESET message') self.initialize_path_queue() rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches # for three seconds and wait_for_message() in a # loop will see it again. reset_thread = threading.Thread(target=wait_for_path_reset) reset_thread.start()
def main(): rospy.init_node('issue_com') pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10) test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10) sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen) #sub = rospy.Subscriber('/joint_states', JointState, listen) pc = PositionCommand() pc.mode = JOINT_SPACE #pc.arm = PositionCommand.LEFT_ARM pc.arm = 1#PositionCommand.RIGHT_ARM pc.data = np.zeros(7) r = rospy.Rate(1) #while not rospy.is_shutdown(): # pub.publish(pc) # r.sleep() # print 'published!' r.sleep() test_pub.publish(Empty()) pub.publish(pc)
def main(): trans= 0 rot = 0 rospy.init_node('odom_sync') listener = tf.TransformListener() pubfiltered = rospy.Publisher('set_pose',PoseWithCovarianceStamped,queue_size = 10) pubodom = rospy.Publisher('mobile_base/commands/reset_odometry', Empty, queue_size = 10) serv = rospy.ServiceProxy("set_offset",SetOdomOffset) rospy.Subscriber('/command_input',UInt8,callback,callback_args=(pubodom,pubfiltered,listener,trans,rot,serv)) rospy.sleep(1) print "done sleeping" while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/base_meas', '/odom_meas',rospy.Time(0)) except: continue rospy.spin()
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, node_name): rospy.init_node(node_name, anonymous=False) try: conf_file = rospy.get_param('~config_file') except KeyError as e: rospy.logerr('You should set the parameter for perception config file...') quit() with open(os.path.abspath(conf_file)) as f: self.conf_data = yaml.load(f.read()) rospy.loginfo('loading perception config file... %d perception(s) exists...'%len(self.conf_data.keys())) for item in self.conf_data.keys(): rospy.loginfo('\033[92m - %s: %d event(s) and %d data(s).\033[0m'%(item, len(self.conf_data[item]['events']), len(self.conf_data[item]['data']))) self.dict_srv_wr = {} self.dict_srv_rd = {} for item in self.conf_data.keys(): if self.conf_data[item].has_key('target_memory'): memory_name = self.conf_data[item]['target_memory'] rospy.loginfo('\033[94m[%s]\033[0m wait for bringup %s...'%(rospy.get_name(), memory_name)) rospy.wait_for_service('/%s/write_data'%memory_name) self.dict_srv_wr[memory_name] = rospy.ServiceProxy('/%s/write_data'%memory_name, WriteData) self.dict_srv_rd[memory_name] = rospy.ServiceProxy('/%s/read_data'%memory_name, ReadData) self.register_data_to_memory(memory_name, item, self.conf_data[item]['data']) self.is_enable_perception = True rospy.Subscriber('%s/start'%rospy.get_name(), Empty, self.handle_start_perception) rospy.Subscriber('%s/stop'%rospy.get_name(), Empty, self.handle_stop_perception) self.pub_event = rospy.Publisher('forwarding_event', ForwardingEvent, queue_size=10) rospy.loginfo('\033[94m[%s]\033[0m initialize perception_base done...'%rospy.get_name())
def __init__(self, filename, effects): self.complete = False self.effects = effects self.filename = filename self.kill_sub = rospy.Subscriber( '/needybot/speech/kill', Empty, self.handle_kill_msg) self.process = None super(SoundProcess, self).__init__()
def handle_kill_msg(self, msg): """ ROS subscription handler the topic ``/needybot/speech/kill`` which takes a ``std_msgs.Empty`` message type. Kills the internal sox subprocess (stops playback). :param: msg -- std_msgs.Empty message type """ if self.process: self.process.kill()
def publish(self, publisher_name): if self.publishers.get(publisher_name) == None: self.publishers[publisher_name] = rospy.Publisher( '/needybot/blackboard/{}'.format(publisher_name), ros_msg.Empty, queue_size=1 ) rospy.sleep(0.1) self.publishers[publisher_name].publish(ros_msg.Empty())
def task_name(self, val): self._task_name = val self.failure_pub = rospy.Publisher( '/needybot/{}/step/failure'.format(self._task_name), ros_msg.Empty, queue_size=1 ) self.success_pub = rospy.Publisher( '/needybot/{}/step/success'.format(self._task_name), ros_msg.Empty, queue_size=1 )
def fail(self): self.failure_pub.publish(ros_msg.Empty())
def succeed(self): self.success_pub.publish(ros_msg.Empty())
def __init__(self): self.initial_poses = {} self.planning_groups_tips = {} self.tf_listener = tf.TransformListener() self.marker_lock = threading.Lock() self.prev_time = rospy.Time.now() self.counter = 0 self.history = StatusHistory(max_length=10) self.pre_pose = PoseStamped() self.pre_pose.pose.orientation.w = 1 self.current_planning_group_index = 0 self.current_eef_index = 0 self.initialize_poses = False self.initialized = False self.parseSRDF() self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String, queue_size=5) self.updatePlanningGroup(0) self.updatePoseTopic(0, False) self.joy_pose_pub = rospy.Publisher("/joy_pose", PoseStamped, queue_size=1) self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty, queue_size=5) self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty, queue_size=5) self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty, queue_size=5) self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty, queue_size=5) self.interactive_marker_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full", InteractiveMarkerInit, self.markerCB, queue_size=1) self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
def __init__(self, limb = "right"): # 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._des_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") rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos) rospy.Subscriber("release_spring", Float32, self._release) rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active) self.max_stiffness = 20 self.time_to_maxstiffness = .3 ######### 0.68 self.t_release = rospy.get_time() self._imp_ctrl_is_active = True for joint in self._limb.joint_names(): self._springs[joint] = 30 self._damping[joint] = 4
def execute(self, userdata): global waypoints self.initialize_path_queue() self.path_ready = False # Start thread to listen for when the path is ready (this function will end then) def wait_for_path_ready(): """thread worker function""" data = rospy.wait_for_message('/path_ready', Empty) rospy.loginfo('Recieved path READY message') self.path_ready = True ready_thread = threading.Thread(target=wait_for_path_ready) ready_thread.start() topic = "/initialpose" rospy.loginfo("Waiting to recieve waypoints via Pose msg on topic %s" % topic) rospy.loginfo("To start following waypoints: 'rostopic pub /path_ready std_msgs/Empty -1'") # Wait for published waypoints while not self.path_ready: try: pose = rospy.wait_for_message(topic, PoseWithCovarianceStamped, timeout=1) except rospy.ROSException as e: if 'timeout exceeded' in e.message: continue # no new waypoint within timeout, looping... else: raise e rospy.loginfo("Recieved new waypoint") waypoints.append(pose) # publish waypoint queue as pose array so that you can see them in rviz, etc. self.poseArray_publisher.publish(convert_PoseWithCovArray_to_PoseArray(waypoints)) # Path is ready! return success and move on to the next state (FOLLOW_PATH) return 'success'
def main(): rospy.init_node('issue_com') pub = rospy.Publisher(TRIAL_COM_TOPIC, TrialCommand, queue_size=10) test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10) sub = rospy.Subscriber(POS_COM_TOPIC, TrialCommand, listen) sub2 = rospy.Subscriber(RESULT_TOPIC, SampleResult, listen_report) #sub = rospy.Subscriber('/joint_states', JointState, listen) tc = TrialCommand() T = 1 tc.controller = get_lin_gauss_test(T=T) tc.T = T tc.frequency = 20.0 # NOTE: ordering of datatypes in state is determined by the order here tc.state_datatypes = [JOINT_ANGLES, JOINT_VELOCITIES] tc.obs_datatypes = tc.state_datatypes tc.ee_points = EE_SITES.reshape(EE_SITES.size).tolist() r = rospy.Rate(1) #while not rospy.is_shutdown(): # pub.publish(pc) # r.sleep() # print 'published!' r.sleep() test_pub.publish(Empty()) pub.publish(tc) rospy.spin()
def new_game(self): rospy.wait_for_service('reset_positions') self.resetStage() self.terminal = 0 self.sendTerminal = 0 newStateMSG = EmptyMsg() self.pub_new_goal_.publish( newStateMSG) cv2.waitKey(30) return self.preprocess(), 0, False
def __init__(self): self.objects = {} self.rate = 100 rospy.Subscriber('/environment/reset', Empty, self.reset_callback, queue_size = 1)
def __init__(self): self.is_rendering = False rospy.loginfo('\033[91m[%s]\033[0m waiting for bringup social_mind...'%rospy.get_name()) self.rd_memory = {} try: rospy.wait_for_service('social_events_memory/read_data') self.rd_memory['social_events_memory'] = rospy.ServiceProxy('social_events_memory/read_data', ReadData) rospy.wait_for_service('environmental_memory/read_data') self.rd_memory['environmental_memory'] = rospy.ServiceProxy('environmental_memory/read_data', ReadData) rospy.wait_for_service('system_events_memory/read_data') self.rd_memory['system_events_memory'] = rospy.ServiceProxy('system_events_memory/read_data', ReadData) except rospy.exceptions.ROSInterruptException as e: rospy.logerr(e) quit() self.renderer_client = actionlib.SimpleActionClient('render_scene', RenderSceneAction) rospy.loginfo('\033[91m[%s]\033[0m waiting for motion_renderer to start...'%rospy.get_name()) try: self.renderer_client.wait_for_server() except rospy.exceptions.ROSInterruptException as e: quit() rospy.Subscriber('reply', Reply, self.handle_domain_reply) self.pub_log_item = rospy.Publisher('log', LogItem, queue_size=10) self.pub_start_speech_recognizer = rospy.Publisher('speech_recognizer/start', Empty, queue_size=1) self.pub_stop_speech_recognizer = rospy.Publisher('speech_recognizer/stop', Empty, queue_size=1) self.pub_start_robot_speech = rospy.Publisher('robot_speech/start', Empty, queue_size=1) self.pub_stop_robot_speech = rospy.Publisher('robot_speech/stop', Empty, queue_size=1) self.is_speaking_started = False rospy.Subscriber('start_of_speech', Empty, self.handle_start_of_speech) rospy.Subscriber('end_of_speech', Empty, self.handle_end_of_speech) # rospy.Subscriber('emotion_status', EmotionStatus, self.handle_emotion_status, queue_size=10) # self.current_emotion = 'neutral' # self.current_emotion_intensity = 1.0 self.pub_set_idle_motion = rospy.Publisher('idle_motion/set_enabled', Bool, queue_size=10) self.pub_set_idle_motion.publish(True) self.scene_queue = Queue.Queue(MAX_QUEUE_SIZE) self.scene_handle_thread = Thread(target=self.handle_scene_queue) self.scene_handle_thread.start() rospy.loginfo("\033[91m[%s]\033[0m initialized." % rospy.get_name())
def __init__(self, start_server=True): # Private properties rospack = rospkg.RosPack() self._package_path = rospack.get_path('needybot_speech') self._cache_dir = rospy.get_param( '/needybot/speech/cache_dir', os.path.join(os.path.realpath(self._package_path), 'cache') ) self._cache_manifest = os.path.join(self._cache_dir, 'manifest.json') self._feedback = SpeechFeedback() self._kill_pub = rospy.Publisher( '/needybot/speech/kill', Empty, queue_size=10) self._result = SpeechResult() self._server = actionlib.SimpleActionServer( 'needybot_speech', SpeechAction, execute_cb=self.execute_cb, auto_start=False) # Public properties self.effects = [ 'ladspa', 'tap_deesser', 'tap_deesser', '-30', '6200', 'pitch', '200', 'contrast', '75', 'norm' ] self.voice = pyvona.create_voice( os.environ.get('IVONA_ACCESS_KEY'), os.environ.get('IVONA_SECRET_KEY')) self.voice.voice_name = rospy.get_param( '/needybot/speech/voice/name', 'Justin') self.voice.speech_rate = rospy.get_param( '/needybot/speech/voice/speech_rate', 'medium') self.voice.codec = rospy.get_param('/needybot/speech/voice/codec', 'ogg') self.sound_process = None self.cleaned_pub = rospy.Publisher( '/needybot/speech/cache/cleaned', Empty, queue_size=10, latch=True ) self.warmed_pub = rospy.Publisher( '/needybot/speech/cache/warmed', Empty, queue_size=10, latch=True ) if not os.path.exists(self._cache_manifest): self.create_cache_manifest() if start_server: self._server.start()
def __init__(self, name): super(Task, self).__init__(name, rospy.Rate(50)) self.nb_blackboard = NeedybotBlackboard() self.lock = threading.Lock() # Flags self.active = False self.completed = False self.did_fail = False self.current_step = None self.name = name self.subscribers = [] self.step_timer = None self.steps = {} self.task_latcher = TaskLatcher() self.task_latcher.register_task() self.step_load_time = None self.paused_time = None self.paused = False self.register_services([ ('abort', AbortTask), ('step_name', Message), ('next_step', NextStep), ('task_payload', TaskPayload), ('reset', Empty), ('status', TaskStatus), ('step', StepTask) ]) self.add_steps([ TaskStep( 'load', failure_step='abort', success_step='complete', entered_handler=self.load_entered ), TaskStep( 'complete', entered_handler=self.complete_entered ), TaskStep( 'abort', entered_handler=self.abort_entered, blocking=True ), ])
def __init__(self, max_random_start, observation_dims, data_format, display): self.max_random_start = max_random_start self.action_size = 28 self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) ##################### # Followings are not used yet self.syncLaser = False self.syncPose = False self.syncGoal = False self.syncDir = False self.sentTime = 0 ##################### self.poseX = 0.0 self.poseY = 0.0 self.prevPoseX = 0.0 self.prevPoseY = 0.0 self.goalX = 0.0 self.goalY = 0.0 self.dir = 0.0 self.prevDist = 0.0 self.terminal = 0 self.sendTerminal = 0 self.clock = Clock() self.lastClock = Clock() rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Publishers: self.pub_action_ = rospy.Publisher("dqn/selected_action",Int8,queue_size=1) self.pub_new_goal_ = rospy.Publisher("dqn/new_goal",EmptyMsg,queue_size=1) self.pub_rew_ = rospy.Publisher("dqn/lastreward",Float32,queue_size=1) # Subscribers: rospy.Subscriber('bridge/laser_image', Image, self.laserCB,queue_size=1) rospy.Subscriber('bridge/current_dir', Float32, self.directionCB,queue_size=1) rospy.Subscriber('bridge/impact', EmptyMsg, self.impactCB,queue_size=1) rospy.Subscriber('bridge/current_pose', Pose, self.positionCB,queue_size=1) rospy.Subscriber('bridge/goal_pose', Pose, self.targetCB,queue_size=1) rospy.Subscriber('clock', Clock, self.stepCB,queue_size=1)
def step(self, action, is_training=False): self.prevPoseX = self.poseX self.prevPoseY = self.poseY if action == -1: # Step with random action action = int(random.random()*(self.action_size)) msg = Int8() msg.data = action self.pub_action_.publish( msg) if self.display: cv2.imshow("Screen", self.screen) #cv2.waitKey(9) dist = (self.poseX - self.goalX)**2 + (self.poseY - self.goalY)**2 reward = (self.prevDist - dist)/10.0 self.prevDist = dist if self.terminal == 1: reward -= 900 #self.new_random_game() if dist < 0.9: reward += 300 newStateMSG = EmptyMsg() self.pub_new_goal_.publish( newStateMSG) # cv2.waitKey(30) # Add whatever info you want info = "" #rospy.loginfo("Episede ended, reward: %g", reward) while(self.clock == self.lastClock): pass self.lastClock = self.clock if self.terminal == 2: self.sendTerminal = 1 if self.terminal == 1: # rewd = Float32() # rewd.data = reward # self.pub_rew_.publish( rewd) self.terminal = 2 return self.screen, reward, self.sendTerminal, info #observation, reward, terminal, info = self.env.step(action) #return self.preprocess(observation), reward, terminal, info
