Python rospy.loginfo() Examples

def __init__(self, goalListX, goalListY, retry, map_frame):
        self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
        self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)   
        # params & variables
        self.goalListX = goalListX
        self.goalListY = goalListY
        self.retry = retry
        self.goalId = 0
        self.goalMsg = PoseStamped()
        self.goalMsg.header.frame_id = map_frame
        self.goalMsg.pose.orientation.z = 0.0
        self.goalMsg.pose.orientation.w = 1.0
        # Publish the first goal
        time.sleep(1)
        self.goalMsg.header.stamp = rospy.Time.now()
        self.goalMsg.pose.position.x = self.goalListX[self.goalId]
        self.goalMsg.pose.position.y = self.goalListY[self.goalId]
        self.pub.publish(self.goalMsg) 
        rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) 
        self.goalId = self.goalId + 1 

def start(self, node_name='polly_node', service_name='polly'):
        """The entry point of a ROS service node.

        Details of the service API can be found in Polly.srv.

        :param node_name: name of ROS node
        :param service_name:  name of ROS service
        :return: it doesn't return
        """
        rospy.init_node(node_name)

        service = rospy.Service(service_name, Polly, self._node_request_handler)

        rospy.loginfo('polly running: {}'.format(service.uri))

        rospy.spin() 

def publish_plan(self):
        """
        publish the global plan
        """
        msg = Path()
        msg.header.frame_id = "/map"
        msg.header.stamp = rospy.Time.now()
        for wp in self._global_plan_world_coord:
            pose = PoseStamped()
            pose.pose.position.x = wp[0].location.x
            pose.pose.position.y = -wp[0].location.y
            pose.pose.position.z = wp[0].location.z
            quaternion = tf.transformations.quaternion_from_euler(
                0, 0, -math.radians(wp[0].rotation.yaw))
            pose.pose.orientation.x = quaternion[0]
            pose.pose.orientation.y = quaternion[1]
            pose.pose.orientation.z = quaternion[2]
            pose.pose.orientation.w = quaternion[3]
            msg.poses.append(pose)

        rospy.loginfo("Publishing Plan...")
        self.waypoint_publisher.publish(msg) 

def scaleWheel():
    rospy.init_node("wheel_scaler")
    rospy.loginfo("wheel_scaler started")
    
    scale = rospy.get_param('distance_scale', 1)
    rospy.loginfo("wheel_scaler scale: %0.2f", scale)
    
    rospy.Subscriber("lwheel", Int16, lwheelCallback)
    rospy.Subscriber("rwheel", Int16, rwheelCallback)
    
    lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10)
    rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) 
    
    ### testing sleep CPU usage
    r = rospy.Rate(50) 
    while not rospy.is_shutdown:
        r.sleep()
        
    rospy.spin() 

def __init__(self):
        FORMAT = pyaudio.paInt16
        CHANNELS = 1
        RATE = 16000
        CHUNK = 4096
        self.audio = pyaudio.PyAudio()
        self.stream = self.audio.open(format=FORMAT, channels=CHANNELS, rate=RATE,
                                      input=True, frames_per_buffer=CHUNK,
                                      stream_callback=self._callback)
        self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.read_list = [self.serversocket]

        self._server_name = rospy.get_param('/dialogflow_client/server_name',
                                            '127.0.0.1')
        self._port = rospy.get_param('/dialogflow_client/port', 4444)

        rospy.loginfo("DF_CLIENT: Audio Server Started!") 

def __init__(self):
    ###############################################
        rospy.init_node("wheel_loopback");
        self.nodename = rospy.get_name()
        rospy.loginfo("%s started" % self.nodename)
        
        self.rate = rospy.get_param("~rate", 200)
        self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
        self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
        self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
        self.latest_motor = 0
        
        self.wheel = 0
        
        rospy.Subscriber('motor', Int16, self.motor_callback)
        
        self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10)
        
    ############################################### 

def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

def initUI(self):      
    #####################################################################    
        
        img_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/../images/crosshair.jpg"
        rospy.loginfo("initUI img_path: %s" % img_path)

        self.statusBar()
        
        self.setStyleSheet("QMainWindow { border-image: url(%s); }" % img_path)
        
                
        self.setGeometry(0, 600, 200, 200)
        self.setWindowTitle('Virtual Joystick')
        self.show()
        self.timer = QtCore.QBasicTimer()
        
        self.statusBar().showMessage('started')
        
    ##################################################################### 

def pubTwist(self):
    #######################################################
        # rospy.loginfo("publishing twist from (%0.3f,%0.3f)" %(self.x,self.y))
        self.twist = Twist()
        self.twist.linear.x = (1-self.y) * (x_max - x_min) + x_min
        self.twist.linear.y = 0
        self.twist.linear.z = 0
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = (1-self.x) * (r_max - r_min) + r_min
        
        if self.twist.linear.x > x_max:
            self.twist.linear.x = x_max
        if self.twist.linear.x < x_min:
            self.twist.linear.x = x_min
        if self.twist.angular.z > r_max:
            self.twist.angular.z = r_max
        if self.twist.angular.z < r_min:
            self.twist.angular.z = r_min
        
        self.pub_twist.publish( self.twist )
        
##########################################################################
########################################################################## 

def spinOnce(self):
    #############################################################
    
        # dx = (l + r) / 2
        # dr = (r - l) / w
            
        self.right = 1.0 * self.dx + self.dr * self.w / 2 
        self.left = 1.0 * self.dx - self.dr * self.w / 2
        # rospy.loginfo("publishing: (%d, %d)", left, right) 
                
        self.pub_lmotor.publish(self.left)
        self.pub_rmotor.publish(self.right)
            
        self.ticks_since_target += 1

    ############################################################# 

def vel_ctrl(self):
        self.lin_vel = self.target_vel.linear.x - self.usv_vel.twist.twist.linear.x
        self.lin_vel = self.lin_vel * self.kp_lin + self.I_lin(self.lin_vel)

        #if self.target_vel.angular.z != self.target_vel_ant.angular.z:
        #    self.I_ant_ang = 0

        if self.target_vel.angular.z == 0:
            self.vel_left = self.lin_vel
            self.vel_right = self.lin_vel
        else: 
            self.ang_vel = self.target_vel.angular.z - self.usv_vel.twist.twist.angular.z
            self.erro = self.ang_vel
            self.ang_vel = self.ang_vel * self.kp_ang + self.I_ang(self.ang_vel)
            self.vel_left = self.lin_vel - self.ang_vel
            self.vel_right = self.lin_vel + self.ang_vel
                   
        self.vel_left = self.sat_thruster(self.vel_left)
        self.vel_right = self.sat_thruster(self.vel_right)
        msg = "atual: {0}; desejada: {1}; erro: {2}; vel_left: {3}; vel_right: {4}; erro_bruto: {5}" .format(self.usv_vel.twist.twist.angular.z, self.target_vel.angular.z, self.ang_vel, self.vel_left, self.vel_right, self.erro)
        rospy.loginfo(msg)

        return [self.vel_left, self.vel_right] 

def loadMap():
	global array, width, height, originX, originY, mymap, minSpeed, maxSpeed
	mynorm = matplotlib.colors.Normalize(vmin=-0.1,vmax=0.1);

	mymap.header.frame_id="odom";
	mymap.info.resolution = resolution;
	mymap.info.width = width;
	mymap.info.height = height;
	mymap.info.origin.position.x = originX;
	mymap.info.origin.position.y = originY;
	mymap.info.origin.position.z = 0;
	mymap.info.origin.orientation.x = 0;
	mymap.info.origin.orientation.y = 0;
	mymap.info.origin.orientation.z = 0;
	mymap.info.origin.orientation.w = 1;
	rospy.loginfo ("################### Preparing map. Size (%d, %d) origin(%d, %d)", width, height, originX, originY)
	for x in xrange(height-1, -1, -1):
		for y in range(0, width):
			mymap.data.append(0);			
	rospy.loginfo("#################### Map loaded!") 

def _call_engine(self, **kw):
        """Call engine to do the job.

        If no output path is found from input, the audio file will be put into /tmp and the file name will have
        a prefix of the md5 hash of the text.

        :param kw: what AmazonPolly needs to synthesize
        :return: response from AmazonPolly
        """
        if 'output_path' not in kw:
            tmp_filename = hashlib.md5(kw['text']).hexdigest()
            tmp_filepath = os.path.join(os.sep, 'tmp', 'voice_{}_{}'.format(tmp_filename, str(time.time())))
            kw['output_path'] = os.path.abspath(tmp_filepath)
        rospy.loginfo('audio will be saved as {}'.format(kw['output_path']))

        return self.engine(**kw) 

def _node_request_handler(self, request):
        """The callback function for processing service request.

        It never raises. If anything unexpected happens, it will return a SynthesizerResponse with the exception.

        :param request: an instance of SynthesizerRequest
        :return: a SynthesizerResponse
        """
        rospy.loginfo(request)
        try:
            kws = self._parse_request_or_raise(request)
            res = self._call_engine(**kws).result

            return SynthesizerResponse(res)
        except Exception as e:
            return SynthesizerResponse('Exception: {}'.format(e)) 

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) 

def on_boxes_state_message(msg):
    #rospy.loginfo("received from test node: " + str(msg))

    str_msg = ""
    exit_flag = False
    for i, pose in enumerate(msg.pose):
        name = msg.name[i]    
        if "block" in name:
            str_msg +=  str(name) + ".y: "+ str(pose.position.y) +"\n"
            if pose.position.y >0.75:
                exit_flag = True
                rospy.logwarn("success, cube properly sent to tray. Ending!")
                break

    rospy.loginfo_throttle(1.0, "[Test Node] "+ str_msg)
    
    if exit_flag:
        exit_properly_runtime_test() 

def destroy(self):
        """
        Cleanup of all ROS publishers
        """
        if self.stack_process and self.stack_process.poll() is None:
            rospy.loginfo("Sending SIGTERM to stack...")
            os.killpg(os.getpgid(self.stack_process.pid), signal.SIGTERM)
            rospy.loginfo("Waiting for termination of stack...")
            self.stack_process.wait()
            time.sleep(5)
            rospy.loginfo("Terminated stack.")

        rospy.loginfo("Stack is no longer running")
        self.world_info_publisher.unregister()
        self.map_file_publisher.unregister()
        self.vehicle_status_publisher.unregister()
        self.vehicle_info_publisher.unregister()
        self.waypoint_publisher.unregister()
        self.stack_process = None
        rospy.loginfo("Cleanup finished") 

def __init__(self):
        self.interrupted = False
        self.detector = None
        rpack = RosPack()
        # UMDL or PMDL file paths along with audio files
        pkg_path = rpack.get_path('dialogflow_ros')
        self.model_path = pkg_path + '/scripts/snowboy/resources/jarvis.umdl'
        ding_path = pkg_path + '/scripts/snowboy/resources/ding.wav'
        # Setup df
        self.df_client = None
        # Setup audio output
        ding = wave.open(ding_path, 'rb')
        self.ding_data = ding.readframes(ding.getnframes())
        self.audio = pyaudio.PyAudio()
        self.stream_out = self.audio.open(
                format=self.audio.get_format_from_width(ding.getsampwidth()),
                channels=ding.getnchannels(), rate=ding.getframerate(),
                input=False, output=True)
        self.last_contexts = []
        rospy.loginfo("HOTWORD_CLIENT: Ready!") 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('odom', Odometry, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.3)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 

def shutdown(self):
        # Stop the robot
        try:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.Publish(Twist())
            rospy.sleep(2)
        except:
            pass
        rospy.loginfo("Shutting down Arduino Node...") 

def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans) 

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.wait_for_service("/ExternalTools/right/PositionKinematicsNode/IKService")

    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 talker():
    pub = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
    rospy.init_node('ctrl_jointState_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    hello_str = JointState()
    hello_str.header = Header()
    hello_str.name = ['fwd_left']
    hello_str.position = [10]
    hello_str.velocity = []
    hello_str.effort = []

    while not rospy.is_shutdown():
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep() 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      self.last_imu_angle = self.imu_angle
      rospy.sleep(10)
      rospy.loginfo("imu_drift:"+str((self.imu_angle-self.last_imu_angle)*180/3.1415926)) 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.1)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 

def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5)
        
        # Wait for the /odom_combined topic to become available
        rospy.wait_for_message('input', PoseWithCovarianceStamped)
        
        # Subscribe to the /odom_combined topic
        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
        
        rospy.loginfo("Publishing combined odometry on /odom_ekf") 

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 shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Cancel any active goals
        self.move_base.cancel_goal()
        rospy.sleep(2)
        # Stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1) 

def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1) 

def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)