我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.loginfo()。
def __init__(self): rospy.init_node('multiplexer_node', anonymous=False) rospy.loginfo('\033[93m[%s]\033[0m wait for bringup social_events_memory...'%rospy.get_name()) rospy.wait_for_service('social_events_memory/read_data') self.rd_event_mem = rospy.ServiceProxy('social_events_memory/read_data', ReadData) rospy.Subscriber('forwarding_event', ForwardingEvent, self.handle_social_events) self.pub_rasing_event = rospy.Publisher('raising_events', RaisingEvents, queue_size=10) self.events_queue = Queue.Queue() self.recognized_words_queue = Queue.Queue() event_period = rospy.get_param('~event_period', 0.5) rospy.Timer(rospy.Duration(event_period), self.handle_trigger_events) rospy.loginfo('\033[93m[%s]\033[0m initialized...'%rospy.get_name()) rospy.spin()
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep() # rate = rospy.Rate(10) # hello_str = "hello world" # rospy.loginfo(hello_str) # pub.publish(hello_str) # rospy.spin() # exit(0)
def set_piksi_settings(self): save_needed = False for s in nested_dict_iter(self.piksi_update_settings): cval = self.piksi_settings[s[0][0]][s[0][1]] if len(cval) != 0: if cval != str(s[1]): rospy.loginfo('Updating piksi setting %s:%s to %s.' % (s[0][0], s[0][1], s[1])) self.piksi_set(s[0][0], s[0][1], s[1]) self.update_dr_param(s[0][0], s[0][1], s[1]) save_needed = True else: rospy.loginfo('Piksi setting %s:%s already set to %s.' % (s[0][0], s[0][1], s[1])) else: rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0])) if self.piksi_save_settings and save_needed: rospy.loginfo('Saving piksi settings to flash') m = MsgSettingsSave() self.piksi_framer(m)
def callback_sbp_base_pos(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_BASE_POS (Sender: %d): %s" % (msg.sender, repr(msg))) if self.send_observations: for s in self.obs_senders: s.send(msg) # publish tf for rtk frame if self.publish_utm_rtk_tf: if not self.proj: self.init_proj((msg.lat, msg.lon)) E,N = self.proj(msg.lon,msg.lat, inverse=False) self.transform.header.stamp = rospy.Time.now() self.transform.transform.translation.x = E self.transform.transform.translation.y = N self.transform.transform.translation.z = -msg.height self.tf_br.sendTransform(self.transform)
def __init__(self): self._read_configuration() if self.show_plots: self._setup_plots() rospy.loginfo("Receiving timestamp messages from {}".format(self.uwb_timestamps_topic)) rospy.loginfo("Publishing multi-range messages to {}".format(self.uwb_multi_range_topic)) rospy.loginfo("Publishing raw multi-range messages to {}".format(self.uwb_multi_range_raw_topic)) rospy.loginfo("Publishing multi-range-with-offsets messages to {}".format( self.uwb_multi_range_with_offsets_topic)) # ROS Publishers self.uwb_pub = rospy.Publisher(self.uwb_multi_range_topic, uwb.msg.UWBMultiRange, queue_size=1) self.uwb_raw_pub = rospy.Publisher(self.uwb_multi_range_raw_topic, uwb.msg.UWBMultiRange, queue_size=1) self.uwb_with_offsets_pub = rospy.Publisher(self.uwb_multi_range_with_offsets_topic, uwb.msg.UWBMultiRangeWithOffsets, queue_size=1) self.uwb_timestamps_sub = rospy.Subscriber(self.uwb_timestamps_topic, uwb.msg.UWBMultiRangeTimestamps, self.handle_timestamps_message) # Variables for rate display self.msg_count = 0 self.last_now = rospy.get_time()
def _read_unit_offsets(self): if not rospy.has_param('~num_of_units'): rospy.logwarn("No unit offset parameters found!") num_of_units = rospy.get_param('~num_of_units', 0) self._unit_offsets = np.zeros((num_of_units, 3)) self._unit_coefficients = np.zeros((num_of_units, 2)) for i in xrange(num_of_units): unit_params = rospy.get_param('~unit_{}'.format(i)) x = unit_params['x'] y = unit_params['y'] z = unit_params['z'] self._unit_offsets[i, :] = [x, y, z] p0 = unit_params['p0'] p1 = unit_params['p1'] self._unit_coefficients[i, :] = [p0, p1] rospy.loginfo("Unit offsets: {}".format(self._unit_offsets)) rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))
def __init__(self): """Initialize tracker. """ self._read_configuration() self.estimates = {} self.estimate_times = {} self.ikf_prev_outlier_flags = {} self.ikf_outlier_counts = {} self.outlier_thresholds = {} rospy.loginfo("Receiving raw multi-range messages from: {}".format(self.uwb_multi_range_topic)) rospy.loginfo("Publishing tracker messages to {}".format(self.uwb_tracker_topic)) rospy.loginfo("Publishing tracker transform as {} -> {}".format(self.tracker_frame, self.target_frame)) # ROS publishers and subscribers self.tracker_frame = self.tracker_frame self.target_frame = self.target_frame self.uwb_pub = rospy.Publisher(self.uwb_tracker_topic, uwb.msg.UWBTracker, queue_size=1) self.tf_broadcaster = tf.TransformBroadcaster() self.uwb_multi_range_sub = rospy.Subscriber(self.uwb_multi_range_topic, uwb.msg.UWBMultiRangeWithOffsets, self.handle_multi_range_message)
def wait_topic_ready(topic_name, url): remote_topic_type = "" while remote_topic_type == "": remote_topic_type = get_remote_topic_type(topic_name, url) #print remote_topic_type+" remote_topic_type" if (remote_topic_type == ""): rospy.loginfo("Failed to get the remote type of topic %s. Retrying...", topic_name) time.sleep(1) local_topic_type = (None, None, None) while local_topic_type[0] == None: local_topic_type = get_topic_type(topic_name) #print str(local_topic_type)+" local_topic_type" if (local_topic_type[0] == None): rospy.loginfo("Failed to get the local type of topic %s. Retrying...", topic_name) time.sleep(1) if topic_name == '/tf': return 'tf/tfMessage' elif remote_topic_type == local_topic_type[0]: return remote_topic_type else: return None
def run(self): self.topic_type = wait_topic_ready(self.topic_name, self.url) #print str(self.topic_type)+" self.topic_type" if not self.topic_type: rospy.logerr('Type of topic %s are not equal in the remote and local sides', self.topic_name) return topic_type_module, topic_type_name = tuple(self.topic_type.split('/')) try: roslib.load_manifest(topic_type_module) msg_module = import_module(topic_type_module + '.msg') self.rostype = getattr(msg_module, topic_type_name) if self.test: self.publisher = rospy.Publisher(self.topic_name + '_rb', self.rostype, queue_size = self.queue_size) else: self.publisher = rospy.Publisher(self.topic_name, self.rostype, queue_size = self.queue_size) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server %s for subscribed topic %s successfully', self.url, self.topic_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Proxy for subscribed topic %s init falied. Reason: Could not find the required resource: %s', self.topic_name, str(e)) except Exception, e: rospy.logerr('Proxy for subscribed topic %s init falied. Reason: %s', self.topic_name, str(e))
def run(self): self.service_type, self.service_args = wait_service_ready(self.service_name, self.url) if not self.service_type: rospy.logerr('Type of service %s are not equal in the remote and local sides', self.service_type) return service_type_module, service_type_name = tuple(self.service_type.split('/')) try: roslib.load_manifest(service_type_module) msg_module = import_module(service_type_module + '.srv') self.srvtype = getattr(msg_module, service_type_name) if self.test: self.caller = rospy.Service(self.service_name + '_rb', self.srvtype, self.callback)#, self.queue_size) else: self.caller = rospy.Service(self.service_name, self.srvtype, self.callback)#, self.queue_size) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server %s for calling service %s successfully', self.url, self.service_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Proxy for service %s init falied. Reason: Could not find the required resource: %s', self.service_name, str(e)) except Exception, e: rospy.logerr('Proxy for service %s init falied. Reason: %s', self.service_name, str(e))
def run(self): self.topic_type = wait_topic_ready(self.topic_name, self.url) if not self.topic_type: rospy.logerr('Type of topic %s are not equal in the remote and local sides', self.topic_name) return topic_type_module, topic_type_name = tuple(self.topic_type.split('/')) try: roslib.load_manifest(topic_type_module) msg_module = import_module(topic_type_module + '.msg') self.rostype = getattr(msg_module, topic_type_name) self.subscriber = rospy.Subscriber(self.topic_name, self.rostype, self.callback) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server for published topic %s successfully', self.topic_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Could not find the required resource %s', str(e)) except Exception, e: rospy.logerr('Proxy for published topic %s init falied. Reason: %s', self.topic_name, str(e))
def __init__(self, arm='right', dataset_id=-1): # MOTION self.promp = ReplayableInteractiveProMP(arm, with_orientation=True, dataset_id=dataset_id) self.arm = ArmCommander(arm, ik='robot') self.init = self.arm.get_current_state() # INTERACTION (TTS + Speech recognition -- Kinect) rospy.loginfo("setting up the kinect...") self.kinect = Kinect2Client('BAXTERFLOWERS.local') self.kinect.tts.params.queue_on() self.kinect.tts.params.set_language('english') self.kinect.tts.start() self.kinect.speech.params.set_confidence(0.3) self.kinect.speech.params.use_system_mic() self.kinect.speech.params.set_vocabulary({'Record a motion': 'record', 'Set a goal': 'goal', 'ready': 'ready', 'stop': 'stop'}, language="en-US") self.kinect.display_speech() success = self.kinect.speech.start() # start with no callback to use the get() method assert success == '', success rospy.loginfo("Kinect started!")
def record_motion(self): for countdown in ['ready?', 3, 2, 1, "go"]: self.say('{}'.format(countdown), blocking=False) rospy.sleep(1) self.arm.recorder.start(10) rospy.loginfo("Recording...") choice = "" while choice != 'stop' and not rospy.is_shutdown(): choice = self.read_user_input(['stop']) joints, eef = self.arm.recorder.stop() self.say('Recorded', blocking=True) if len(joints.joint_trajectory.points) == 0: self.say('This demo is empty, please retry') else: target_id = self.promp.add_demonstration(joints, eef) self.say('Added to Pro MP {}'.format(target_id), blocking=False)
def SpeechUpdateGoal(self, SpeechGoal): rospy.loginfo("input = %s", str(SpeechGoal.data)) if self.locations.has_key(str(SpeechGoal.data)): rospy.loginfo("location = %s", str(SpeechGoal.data)) self.goal = MoveBaseGoal() self.goal.target_pose.pose = self.locations[str(SpeechGoal.data)] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(SpeechGoal.data)) # Start the robot toward the next location self.move_base.send_goal(self.goal) else: rospy.loginfo("No such waypoint")
def test_light_interface(light_name='head_green_light'): """Blinks a desired Light on then off.""" l = Lights() rospy.loginfo("All available lights on this robot:\n{0}\n".format( ', '.join(l.list_all_lights()))) rospy.loginfo("Blinking Light: {0}".format(light_name)) on_off = lambda x: 'ON' if l.get_light_state(x) else 'OFF' rospy.loginfo("Initial state: {0}".format(on_off(light_name))) # turn on light l.set_light_state(light_name, True) rospy.sleep(1) rospy.loginfo("New state: {0}".format(on_off(light_name))) # turn off light l.set_light_state(light_name, False) rospy.sleep(1) rospy.loginfo("New state: {0}".format(on_off(light_name))) # turn on light l.set_light_state(light_name, True) rospy.sleep(1) rospy.loginfo("New state: {0}".format(on_off(light_name))) # reset output l.set_light_state(light_name, False) rospy.sleep(1) rospy.loginfo("Final state: {0}".format(on_off(light_name)))
def main(): """RSDK Inverse Kinematics Example A simple example of using the Rethink Inverse Kinematics Service which returns the joint angles and validity for a requested Cartesian Pose. Run this example, the example will use the default limb and call the Service with a sample Cartesian Pose, pre-defined in the example code, printing the response of whether a valid joint solution was found, and if so, the corresponding joint angles. """ rospy.init_node("rsdk_ik_service_client") if ik_service_client(): rospy.loginfo("Simple IK call passed!") else: rospy.logerr("Simple IK call FAILED") if ik_service_client(use_advanced_options=True): rospy.loginfo("Advanced IK call passed!") else: rospy.logerr("Advanced IK call FAILED")
def main(): """RSDK Forward Kinematics Example A simple example of using the Rethink Forward Kinematics Service which returns the Cartesian Pose based on the input joint angles. Run this example, the example will use the default limb and call the Service with a sample Joint Position, pre-defined in the example code, printing the response of whether a valid Cartesian solution was found, and if so, the corresponding Cartesian pose. """ rospy.init_node("rsdk_fk_service_client") if fk_service_client(): rospy.loginfo("Simple FK call passed!") else: rospy.logerr("Simple FK call FAILED")
def start_server(limb, rate, mode, valid_limbs): rospy.loginfo("Initializing node... ") rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format( mode, "" if limb == 'all_limbs' else "_" + limb,)) rospy.loginfo("Initializing joint trajectory action server...") robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() config_module = "intera_interface.cfg" if mode == 'velocity': config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"]) elif mode == 'position': config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"]) else: config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"]) cfg = importlib.import_module('.'.join([config_module,config_name])) dyn_cfg_srv = Server(cfg, lambda config, level: config) jtas = [] if limb == 'all_limbs': for current_limb in valid_limbs: jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv, rate, mode)) else: jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode)) def cleanup(): for j in jtas: j.clean_shutdown() rospy.on_shutdown(cleanup) rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit") rospy.spin()
def main(): parser = argparse.ArgumentParser() parser.add_argument("-t", "--timeout", type=lambda t:abs(float(t)), default=60.0, help="[in seconds] Time to wait for joints to home") parser.add_argument("-m", "--mode", type=str.upper, default="AUTO", choices=['AUTO', 'MANUAL'], help="Mode to home the robot's joints") enable_parser = parser.add_mutually_exclusive_group(required=False) enable_parser.add_argument("-e", "--enable", action='store_true', dest='enable', help="Try to enable the robot before homing.") enable_parser.add_argument("-n", "--no-enable", action='store_false', dest='enable', help="Avoid trying to enable the robot before homing.") enable_parser.set_defaults(enable=True) args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('home_joints_node') if args.enable: rs = intera_interface.RobotEnable(CHECK_VERSION) rs.enable() cmd_mode = HomingCommand.MANUAL if args.mode == 'MANUAL' else HomingCommand.AUTO rospy.loginfo("Homing joints in '{0}' mode".format(args.mode.capitalize())) home_jnts = HomeJoints() state = home_jnts.home_robot(mode=cmd_mode, timeout=args.timeout) rospy.loginfo("{0} in homing the robot's joints".format("Succeeded" if state else "Failed"))
def _select_play(self): if self._play_termination: # ????Role????? for role in self._play.roles: role.behavior.reset() # Extract possible plays from playbook possible_plays = [] for play in PlayBook.book: if WorldModel.situations[play.applicable]: possible_plays.append(play) # TODO(Asit) select a play randomly if possible_plays: self._play = possible_plays[0] else: self._play = PlayDummy() self._play_past_time = rospy.get_time() self._play_termination = False rospy.loginfo('play reset')
def __init__(self): # initialize a socket self.host = rospy.get_param('~server_address', '127.0.0.1') self.port = rospy.get_param('~server_port', 20011) self.friend_color = rospy.get_param('/friend_color', 'yellow') rospy.loginfo('server address is set to [' + self.host + ']') rospy.loginfo('server port is set to [' + str(self.port) + ']') rospy.loginfo('team color is set to [' + self.friend_color + ']') self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # make subscribers self.subscribers = [] for i in xrange(12): topic = "/robot_" + str(i) + "/robot_commands" self.subscribers.append( rospy.Subscriber( topic, robot_commands, self.sendCommands, callback_args=i))
def move(self, goal): # Send the goal pose to the MoveBaseAction server self.move_base.send_goal(goal) # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!")
def __init__(self): rospy.init_node('gaze', anonymous=False) self.lock = threading.RLock() with self.lock: self.current_state = GazeState.IDLE self.last_state = self.current_state # Initialize Variables self.glance_timeout = 0 self.glance_timecount = 0 self.glance_played = False self.idle_timeout = 0 self.idle_timecount = 0 self.idle_played = False rospy.loginfo('\033[92m[%s]\033[0m waiting for bringup social_mind...'%rospy.get_name()) rospy.wait_for_service('environmental_memory/read_data') rospy.wait_for_service('social_events_memory/read_data') self.rd_memory = {} self.rd_memory['environmental_memory'] = rospy.ServiceProxy('environmental_memory/read_data', ReadData) self.rd_memory['social_events_memory'] = rospy.ServiceProxy('social_events_memory/read_data', ReadData) rospy.Subscriber('raising_events', RaisingEvents, self.handle_raising_events) rospy.Subscriber('gaze_focusing', String, self.handle_gaze_focusing) self.pub_gaze_cmd = rospy.Publisher('gaze_command', GazeCommand, queue_size=10) self.pub_viz_gaze_cmd = rospy.Publisher('visualization_gaze_cmd', PointStamped, queue_size=10) rospy.Timer(rospy.Duration(GAZE_CONTROLLER_PERIOD), self.handle_gaze_controller) rospy.loginfo('\033[92m[%s]\033[0m initialized...'%rospy.get_name()) rospy.spin()
def __init__(self): rospy.init_node('fake_renderer', anonymous=True) try: topic_name = rospy.get_param('~action_name') except KeyError as e: print('[ERROR] Needed parameter for (topic_name)...') quit() if 'render_gesture' in rospy.get_name(): self.GetInstalledGesturesService = rospy.Service( "get_installed_gestures", GetInstalledGestures, self.handle_get_installed_gestures ) self.motion_list = { 'neutral': ['neutral_motion1'], 'encourge': ['encourge_motion1'], 'attention': ['attention_motion1'], 'consolation': ['consolation_motion1'], 'greeting': ['greeting_motion1'], 'waiting': ['waiting_motion1'], 'advice': ['advice_motion1'], 'praise': ['praise_motion1'], 'command': ['command_motion1'], } self.server = actionlib.SimpleActionServer( topic_name, RenderItemAction, self.execute_callback, False) self.server.start() rospy.loginfo('[%s] initialized...' % rospy.get_name()) rospy.spin()
def connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type, multiplier=1, deadband=0 ): rospy.loginfo("Connecting topic {} to topic {}".format( src_topic, dest_topic )) pub = rospy.Publisher(dest_topic, dest_topic_type, queue_size=10) def callback(src_item): val = src_item.data val *= multiplier if dest_topic_type == Bool: val = (val > deadband) dest_item = dest_topic_type(val) pub.publish(dest_item) sub = rospy.Subscriber(src_topic, src_topic_type, callback) return sub, pub
def init_subscribers(self, recipe_phases): ALL_SUBSCRIBER_LIST = { variable.name: rospy.Subscriber( "{}/desired".format(variable.name), get_message_class(variable.type), self.callback) for variable in VALID_VARIABLES } VARIABLES = list(recipe_phases[0]['step'].keys()) # Select the variables in the 1st phase subs = [ALL_SUBSCRIBER_LIST[variable] for variable in VARIABLES] # Initiate all the subscribers for each variable listed in the recipe #air_temp_sub = ALL_SUBSCRIBER_LIST['air_temperature'] #Not working, since the specified recpie name is not in the DB. # def test_run_recipe(self): # self.init_subscribers(MOCK_RECIPE_FLEXFORMAT_A['phases']) # rospy.sleep(10) # Wait for subscribers to spin up # rospy.loginfo(rosservice.get_service_list()) # self.start_recipe(MOCK_RECIPE_FLEXFORMAT_A['_id']) # rospy.sleep(21) #------------------------------------------------------------------------------
def __init__(self): self.air_temp = None self.humidity = None self.co2 = None self.o2 = None self.water_temp = None self.ph = None self.ec = None self.desired_temp = 25 self.desired_hum = 50 self.cmd_temp = 0 self.cmd_hum = 0 self.white = [255,255,255] self.black = [0,0,0] self.event_queue = None rospy.loginfo("Initializing touchscreen") pygame.init() # pygame.mouse.set_visible(False) self.screen = pygame.display.set_mode((WIDTH,HEIGHT),pygame.NOFRAME)
def clean_cache(self): """ This method completely cleans the cache directory of this ``SpeechActionServer`` instance. Use with care. """ rospy.loginfo('Purging speech cache...') for f in os.listdir(self._cache_dir): if f == '.gitkeep': continue try: p = os.path.join(self._cache_dir, f) if os.path.isfile(p): os.unlink(p) except Exception as e: rospy.logerr(e) rospy.loginfo('Speech cache has been purged.') self.cleaned_pub.publish()
def warm_cache(self): """ Collects all dialog in your manifest and stores them in the cache directory as well as creating entries in the cache manifest. """ rospy.loginfo('Warming speech cache...') m = rospy.get_param('/needybot/speech/dialog') for k, v in m.iteritems(): goal = SpeechGoal(key=k) gen = self.generate_cache_payload(goal) payload = self.fetch_from_cache(k, gen.get('hash')) if payload is None: payload = gen if self.validate_payload(payload): self.add_to_cache(payload) self.generate_raw_speech(payload) else: raise TypeError('Invalid payload send to speech server') rospy.loginfo('Speech cache is ready.') self.warmed_pub.publish()
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
def connectNaoQi(self): rospy.loginfo("Exploration Node Connecting to NaoQi at %s:%d", self.pip, self.pport) self.navigation = self.get_proxy("ALNavigation") self.motion = self.get_proxy("ALMotion") if self.navigation is None or self.motion is None: rospy.logerr("Unable to reach ALMotion and ALNavigation.") exit(0) version_array = self.get_proxy("ALSystem").systemVersion().split('.') if len(version_array) < 3: rospy.logerr("Unable to deduce the system version.") exit(0) version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2])) min_version = (2, 5, 2) if version_tuple < min_version: rospy.logerr("Naoqi version " + str(min_version) + " required for localization. Yours is " + str(version_tuple)) exit(0)
def connectNaoQi(self): rospy.loginfo("Node Loading map at %s:%d", self.pip, self.pport) self.navigation = self.get_proxy("ALNavigation") if self.navigation is None: rospy.logerr("Unable to reach ALNavigation.") exit(0) version_array = self.get_proxy("ALSystem").systemVersion().split('.') if len(version_array) < 3: rospy.logerr("Unable to deduce the system version.") exit(0) version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2])) min_version = (2, 5, 2) if version_tuple < min_version: rospy.logerr("Naoqi version " + str(min_version) + " required for localization. Yours is " + str(version_tuple)) exit(0)
def learnMap(self, radius): '''Learning a new map and saving it Input : maximal radius to explore (in meters) ''' #exploration if self.tts is not None: self.tts.post.say(str("starting exploration")) rospy.loginfo("Starting exploration in a radius of %d m", radius) self.setThresholds(0.2, 0.08) self.navigation.explore(radius) self.navigation.stopExploration() rospy.loginfo("Exploration has finished") #saving exploration self.pathtomap = self.navigation.saveExploration() rospy.loginfo("Path to the map: %s", self.pathtomap) if self.tts is not None: self.tts.post.say(str("saving the map")) self.setThresholds(0.4, 0.1)
def __init__(self): NaoqiNode.__init__(self, 'nao_octomap') if self.get_version() < LooseVersion('2.0'): rospy.loginfo('NAOqi version < 2.0, Octomap is not used') exit(0) proxy = self.get_proxy("ALNavigation") if proxy is None: rospy.loginfo('Could not get access to the ALNavigation proxy') exit(1) # Create ROS publisher self.pub = rospy.Publisher("octomap", Octomap, latch = True, queue_size=1) self.fps = 1 rospy.loginfo("nao_octomap initialized")
def callback_external(self, msg, **metadata): if self.debug: rospy.loginfo("Received external SBP msg.") if self.piksi_framer: self.piksi_framer(msg, **metadata) else: rospy.logwarn("Received external SBP msg, but Piksi not connected.")
def set_serial_number(self, serial_number): rospy.loginfo("Connected to piksi #%d" % serial_number) self.serial_number = serial_number self.diag_updater.setHardwareID("Piksi %d" % serial_number)
def callback_sbp_startup(self, msg, **metadata): rospy.loginfo('Piksi startup packet received: %s' % repr(msg))
def callback_sbp_gps_time(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" % (msg.sender, repr(msg))) out = TimeReference() out.header.frame_id = self.frame_id out.header.stamp = rospy.Time.now() out.time_ref = ros_time_from_sbp_time(msg) out.source = "gps" self.pub_time.publish(out)
def callback_sbp_dops(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_DOPS (Sender: %d): %s" % (msg.sender, repr(msg))) self.last_dops = msg
def callback_sbp_vel(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_VEL_NED (Sender: %d): %s" % (msg.sender, repr(msg))) self.last_vel = msg self.publish_odom()
def callback_sbp_baseline(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_BASELINE_NED (Sender: %d): %s" % (msg.sender, repr(msg))) if self.publish_rtk_child_tf: self.base_link_transform.header.stamp = rospy.Time.now() self.base_link_transform.transform.translation.x = msg.e/1000.0 self.base_link_transform.transform.translation.y = msg.n/1000.0 self.base_link_transform.transform.translation.z = -msg.d/1000.0 self.tf_br.sendTransform(self.base_link_transform) self.last_baseline = msg self.publish_odom()
def callback_sbp_ephemeris(self, msg, **metadata): if self.debug: rospy.loginfo("Received SBP_MSG_EPHEMERIS (Sender: %d): %s" % (msg.sender, repr(msg))) if not hasattr(self, 'eph_msg'): self.eph_msg = Ephemeris() self.eph_msg.header.stamp = rospy.Time.now() self.eph_msg.header.frame_id = self.frame_id self.eph_msg.tgd = msg.tgd self.eph_msg.c_rs = msg.c_rs self.eph_msg.c_rc = msg.c_rc self.eph_msg.c_uc = msg.c_uc self.eph_msg.c_us = msg.c_us self.eph_msg.c_ic = msg.c_ic self.eph_msg.c_is = msg.c_is self.eph_msg.dn = msg.dn self.eph_msg.m0 = msg.m0 self.eph_msg.ecc = msg.ecc self.eph_msg.sqrta = msg.sqrta self.eph_msg.omega0 = msg.omega0 self.eph_msg.omegadot = msg.omegadot self.eph_msg.w = msg.w self.eph_msg.inc = msg.inc self.eph_msg.inc_dot = msg.inc_dot self.eph_msg.af0 = msg.af0 self.eph_msg.af1 = msg.af1 self.eph_msg.af2 = msg.af2 self.eph_msg.toe_tow = msg.toe_tow self.eph_msg.toe_wn = msg.toe_wn self.eph_msg.toc_tow = msg.toc_tow self.eph_msg.toc_wn = msg.toc_wn self.eph_msg.valid = msg.valid self.eph_msg.healthy = msg.healthy self.eph_msg.sid.sat = msg.sid.sat self.eph_msg.sid.band = msg.sid.band self.eph_msg.sid.constellation = msg.sid.constellation self.eph_msg.iode = msg.iode self.eph_msg.iodc = msg.iodc self.pub_eph.publish(m)
def spin(self): reconnect_delay = 1.0 while not rospy.is_shutdown(): try: rospy.loginfo("Connecting to SwiftNav Piksi on port %s" % self.piksi_port) self.connect_piksi() while not rospy.is_shutdown(): rospy.sleep(0.05) if not self.piksi.is_alive(): raise IOError self.diag_updater.update() self.check_timeouts() break # should only happen if rospy is trying to shut down except IOError as e: rospy.logerr("IOError") self.disconnect_piksi() except SystemExit as e: rospy.logerr("Unable to connect to Piksi on port %s" % self.piksi_port) self.disconnect_piksi() except: # catch *all* exceptions e = sys.exc_info()[0] rospy.logerr("Uncaught error: %s" % repr(e)) self.disconnect_piksi() rospy.loginfo("Attempting to reconnect in %fs" % reconnect_delay) rospy.sleep(reconnect_delay)
def initial_guess(self, ranges): """Computes an initial position guess based on range measurements. The initial position is computed using Gauss-Newton method. The behavior can be modified with some parameters: `self.initial_guess_...`. Args: ranges (list of floats): Range measurements. Returns: initial_state (numpy.ndarray): Initial state vector (velocity components are zero). """ num_of_units = len(ranges) position = self.initial_guess_position H = np.zeros((num_of_units, position.size)) z = np.zeros((num_of_units, 1)) h = np.zeros((num_of_units, 1)) residuals = np.zeros((num_of_units, 1)) for i in xrange(self.initial_guess_iterations): self._compute_measurements_and_jacobians(ranges, position, h, H, z) new_residuals = z - h position = position + np.dot(self._solve_equation_least_squares(np.dot(H.T, H), H.T), new_residuals) if np.sum((new_residuals - residuals) ** 2) < self.initial_guess_tolerance: break residuals = new_residuals rospy.loginfo('initial guess residuals: {}'.format(residuals)) if np.any(np.abs(residuals) > self.initial_guess_residuals_threshold): # This initial guess is not good enough return None initial_state = np.zeros((6, 1)) initial_state[0:3] = position return initial_state
def update_estimate(self, multi_range_msg): """Update tracker based on a multi-range message. Updates estimate and timestamp in the `self.estimate` and `self.estimate_times` dictionaries. Args: multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. Returns: new_estimate (StateEstimate): Updated position estimate. """ estimate_id = (multi_range_msg.address, multi_range_msg.remote_address) if estimate_id not in self.estimates: initial_state = self.initial_guess(multi_range_msg.ranges) if initial_state is None: return None self.initialize_estimate(estimate_id, initial_state) current_time = rospy.get_time() timestep = current_time - self.estimate_times[estimate_id] estimate = self.estimates[estimate_id] new_estimate, outlier_flag = self.update_filter(timestep, estimate, multi_range_msg.ranges) if not outlier_flag: self.estimates[estimate_id] = new_estimate self.estimate_times[estimate_id] = current_time if self.ikf_prev_outlier_flags[estimate_id]: self.ikf_prev_outlier_flags[estimate_id] = False # If too many outliers are encountered in a row the estimate is deleted. # This will lead to a new initial guess for the next multi-range message. if outlier_flag: if not self.ikf_prev_outlier_flags[estimate_id]: self.ikf_prev_outlier_flags[estimate_id] = True self.ikf_outlier_counts[estimate_id] = 0 self.ikf_outlier_counts[estimate_id] += 1 if self.ikf_outlier_counts[estimate_id] >= self.ikf_max_outlier_count: del self.estimates[estimate_id] rospy.loginfo('Too many outliers in a row. Resetting estimate for address={}, remote_address={}'.format( multi_range_msg.address, multi_range_msg.remote_address )) return new_estimate
def get_remote_topic_type(topic_name, url): while True: try: ws = websocket.create_connection(url) break except Exception, e: rospy.loginfo('Create connection to Rosbridge server %s failed, retrying. Reason: %s', url, str(e)) time.sleep(2) try: # get topic type ws.send(json.dumps({ 'op': 'call_service', 'service': '/rosapi/topic_type', 'args': [topic_name] })) x = json.loads(ws.recv()) assert x['service'] == '/rosapi/topic_type' ws.close() if x['result']: return x['values']['type'] else: return "" except Exception, e: rospy.logerr('Get the type of topic %s from Rosbridge server %s failed. Reason: %s', topic_name, url, str(e)) ws.close() return ""
def get_remote_service_info(service_name, url): while True: try: ws = websocket.create_connection(url) break except Exception, e: rospy.loginfo('Create connection to Rosbridge server %s failed, retrying. Reason: %s', url, str(e)) time.sleep(2) try: # get topic type ws.send(json.dumps({ 'op': 'call_service', 'service': '/rosapi/service_type', 'args': [service_name] })) x = json.loads(ws.recv()) assert x['service'] == '/rosapi/service_type' ws.close() if x['result']: return x['values']['type'] else: return "" except Exception, e: rospy.logerr('Get the type of service %s from Rosbridge server %s failed. Reason: %s', service_name, url, str(e)) ws.close() return ""
