Python naoqi_node.NaoqiNode类代码示例

    def __init__(self):
        NaoqiNode.__init__(self, 'naoqi_moveto_listener')
        self.connectNaoQi()
        self.listener = tf.TransformListener()

        self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goalCB)
        self.cvel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cvelCB)

    def __init__( self ):

        #Initialisation
        NaoqiNode.__init__( self, self.NODE_NAME )

        #Proxy to interface with LEDs
        self.proxy = self.get_proxy( "ALLeds" )

        #Seed python's random number generator
        random.seed( rospy.Time.now().to_nsec() )

        #Create a subscriber for the fade_rgb topic
        self.subscriber = rospy.Subscriber(
            "fade_rgb",
            FadeRGB,
            self.fade_rgb)

        #Prepare and start actionlib server
        self.actionlib = actionlib.SimpleActionServer(
            "blink",
            BlinkAction,
            self.run_blink,
            False
            )
        self.actionlib.start()

    def __init__( self ):

        #Initialisation
        NaoqiNode.__init__( self, self.NODE_NAME )

        #We need this variable to be able to call stop behavior when preempted
        self.behavior = None
        self.lock = threading.RLock()

        #Proxy for listingBehaviors and stopping them
        self.behaviorProxy = self.get_proxy( "ALBehaviorManager" )
        if self.behaviorProxy is None:
            exit(1)

        # Register ROS services
        self.getInstalledBehaviorsService = rospy.Service(
            "get_installed_behaviors",
            GetInstalledBehaviors,
            self.getInstalledBehaviors
            )

        #Prepare and start actionlib server
        self.actionlibServer = actionlib.SimpleActionServer(
            "run_behavior",
            RunBehaviorAction,
            self.runBehavior,
            False
            )

        self.actionlibServer.register_preempt_callback( self.stopBehavior )

        self.actionlibServer.start()

    def __init__(self):
        NaoqiNode.__init__(self, 'naoqi_joint_states')

        self.connectNaoQi()

        # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
        self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))

        self.dataNamesList = ["DCM/Time",
                                "Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
                                "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
                                "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
                                "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
                                "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
                                "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]


        tf_prefix_param_name = rospy.search_param('tf_prefix')
        if tf_prefix_param_name:
            self.tf_prefix = rospy.get_param(tf_prefix_param_name)
        else:
            self.tf_prefix = ""

        self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
        if not(self.base_frameID[0] == '/'):
            self.base_frameID = self.tf_prefix + '/' + self.base_frameID

        # use sensor values or commanded (open-loop) values for joint angles
        self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
        # init. messages:
        self.torsoOdom = Odometry()
        self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
        if not(self.torsoOdom.header.frame_id[0] == '/'):
            self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id

        self.torsoIMU = Imu()
        self.torsoIMU.header.frame_id = self.base_frameID
        self.jointState = JointState()
        self.jointState.name = self.motionProxy.getJointNames('Body')
        self.jointStiffness = JointState()
        self.jointStiffness.name = self.motionProxy.getJointNames('Body')

        # simluated model misses some joints, we need to fill:
        if (len(self.jointState.name) == 22):
            self.jointState.name.insert(6,"LWristYaw")
            self.jointState.name.insert(7,"LHand")
            self.jointState.name.append("RWristYaw")
            self.jointState.name.append("RHand")

        msg = "Nao joints found: "+ str(self.jointState.name)
        rospy.logdebug(msg)

        self.torsoOdomPub = rospy.Publisher("odom_raw", Odometry, queue_size=1)
        self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=1)
        self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=1)
        self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState, queue_size=1)

        self.tf_br = tf.TransformBroadcaster()

        rospy.loginfo("nao_joint_states initialized")

 def __init__(self, moduleName):
     # ROS initialization
     NaoqiNode.__init__(self, Constants.NODE_NAME )
     
     # NAOQi initialization
     self.ip = ""
     self.port = 10601
     self.moduleName = moduleName
     self.init_almodule()
     
     #~ Variable initialization
     self.faces = FaceDetected()  
     face_detection_enabled = False
     motion_detection_enabled = False
     landmark_detection_enabled = False
          
     #~ ROS initializations
     self.subscribeFaceSrv = rospy.Service("nao_vision/face_detection/enable", Empty, self.serveSubscribeFaceSrv)
     self.unsubscribeFaceSrv = rospy.Service("nao_vision/face_detection/disable", Empty, self.serveUnsubscribeFaceSrv)
     self.subscribeMotionSrv = rospy.Service("nao_vision/motion_detection/enable", Empty, self.serveSubscribeMotionSrv)
     self.unsubscribeMotionSrv = rospy.Service("nao_vision/motion_detection/disable", Empty, self.serveUnsubscribeMotionSrv)
     self.subscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/enable", Empty, self.serveSubscribeLandmarkSrv)
     self.unsubscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/disable", Empty, self.serveUnsubscribeLandmarkSrv)
     
     self.subscribe()
     
     rospy.loginfo("nao_vision_interface initialized")

    def __init__(self):
        NaoqiNode.__init__(self, 'nao_footsteps')

        self.connectNaoQi()

        # initial stiffness (defaults to 0 so it doesn't strain the robot when
        # no teleoperation is running)
        # set to 1.0 if you want to control the robot immediately
        initStiffness = rospy.get_param('~init_stiffness', 0.0)

        # TODO: parameterize
        if initStiffness > 0.0 and initStiffness <= 1.0:
            self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)

        # last: ROS subscriptions (after all vars are initialized)
        rospy.Subscriber("footstep", StepTarget, self.handleStep, queue_size=50)

        # ROS services (blocking functions)
        self.stepToSrv = rospy.Service("footstep_srv", StepTargetService,
                                       self.handleStepSrv)
        self.clipSrv = rospy.Service("clip_footstep_srv", ClipFootstep,
                                     self.handleClipSrv)

    	# Initialize action server
        self.actionServer = actionlib.SimpleActionServer(
            "footsteps_execution",
            ExecFootstepsAction,
            execute_cb=self.footstepsExecutionCallback,
            auto_start=False)
        self.actionServer.start()

        rospy.loginfo("nao_footsteps initialized")

 def __init__(self):
     NaoqiNode.__init__(self, 'naoqi_background_movement')
     self.connectNaoQi()
     
     self.SetEnabledSrv = rospy.Service("background_movement/set_enabled", SetBool, self.handleSetEnabledSrv)
     self.IsEnabledSrv = rospy.Service("background_movement/is_enabled", Trigger, self.handleIsEnabledSrv)
     rospy.loginfo("naoqi_background_movement initialized")

    def __init__(self):
        NaoqiNode.__init__(self, 'naoqi_pod_node')

        self.markerpublisher = rospy.Publisher('naoqi_pod_publisher', PoseWithConfidenceStamped, queue_size=5)
        self.rate = rospy.Rate(2)
        self.init = False
        self.connectNaoQi()

    def __init__(self):
        NaoqiNode.__init__(self, 'nao_walker')

        self.connectNaoQi()

        # walking pattern params:
        self.stepFrequency = rospy.get_param('~step_frequency', 0.5)

        self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
        self.needsStartWalkPose = True

        # other params
        self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
        # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
        # set to 1.0 if you want to control the robot immediately
        initStiffness = rospy.get_param('~init_stiffness', 0.0)

        self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
        rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
        if self.useFootGaitConfig:
            self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
        else:
            self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")

        # TODO: parameterize
        if initStiffness > 0.0 and initStiffness <= 1.0:
            self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)

        try:
            enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
            self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
            if enableFootContactProtection:
                rospy.loginfo("Enabled foot contact protection")
            else:
                rospy.loginfo("Disabled foot contact protection")
        except KeyError:
            # do not change foot contact protection
            pass

        # last: ROS subscriptions (after all vars are initialized)
        rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
        rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
        rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)

        # Create ROS publisher for speech
        self.pub = rospy.Publisher("speech", String, latch = True)

        # ROS services (blocking functions)
        self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
        self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
        self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
        self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
        self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
        self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
        self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)

        self.say("Walker online")

        rospy.loginfo("nao_walker initialized")

    def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10):
        NaoqiNode.__init__(self, "sonar_publisher")
        self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
        self.connectNaoQi()

        memory_key = rospy.get_param("~memory_key")
        frame_id = rospy.get_param("~frame_id")
        self.sonarSensor = SonarSensor( memory_key, frame_id, "sonar" )
        self.publisher = rospy.Publisher( self.sonarSensor.rosTopic, Range, queue_size=5)

    def __init__(self, sonarSensorList, param_sonar_rate="~sonar_rate", sonar_rate=40):
        NaoqiNode.__init__(self, "sonar_publisher")
        self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
        self.connectNaoQi()

        self.sonarSensorList = sonarSensorList
        self.publisherList = []
        for sonarSensor in sonarSensorList:
            self.publisherList.append(
                        rospy.Publisher(sonarSensor.rosTopic, Range, queue_size=5))

 def __init__(self):
     NaoqiNode.__init__(self, 'naoqi_localization')
     self.navigation = None
     self.connectNaoQi()
     self.publishers = {}
     self.publishers["uncertainty"] = rospy.Publisher('localization_uncertainty', Marker)
     self.publishers["map_tf"] = rospy.Publisher('/tf', TFMessage, latch=True)
     self.publishers["map"] = rospy.Publisher('naoqi_exploration_map', OccupancyGrid , queue_size=None)
     self.publishers["exploration_path"] = rospy.Publisher('naoqi_exploration_path', Path, queue_size=None)
     self.frequency = 2
     self.rate = rospy.Rate(self.frequency)

    def __init__(self, pointcloud=True, laserscan=False):
        self.pointcloud = pointcloud
        self.laserscan = laserscan

        NaoqiNode.__init__(self, 'pepper_node')

        # ROS initialization;
        self.connectNaoQi()

        # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
        self.laserRate = rospy.Rate(rospy.get_param(
                            self.PARAM_LASER_RATE,
                            self.PARAM_LASER_RATE_DEFAULT))

        self.laserShovelFrame = rospy.get_param(
                                    self.PARAM_LASER_SHOVEL_FRAME,
                                    self.PARAM_LASER_SHOVEL_FRAME_DEFAULT)
        self.laserGroundLeftFrame = rospy.get_param(
                                        self.PARAM_LASER_GROUND_LEFT_FRAME,
                                        self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT)
        self.laserGroundRightFrame = rospy.get_param(
                                        self.PARAM_LASER_GROUND_RIGHT_FRAME,
                                        self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT)
        self.laserSRDFrontFrame = rospy.get_param(
                                        self.PARAM_LASER_SRD_FRONT_FRAME,
                                        self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT)
        self.laserSRDLeftFrame = rospy.get_param(
                                        self.PARAM_LASER_SRD_LEFT_FRAME,
                                        self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT)
        self.laserSRDRightFrame = rospy.get_param(
                                        self.PARAM_LASER_SRD_RIGHT_FRAME,
                                        self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT)

        if self.pointcloud:
            self.pcShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'pointcloud', PointCloud2, queue_size=1)
            self.pcGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'pointcloud', PointCloud2, queue_size=1)
            self.pcGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'pointcloud', PointCloud2, queue_size=1)
            self.pcSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'pointcloud', PointCloud2, queue_size=1)
            self.pcSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'pointcloud', PointCloud2, queue_size=1)
            self.pcSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'pointcloud', PointCloud2, queue_size=1)

        if self.laserscan:
            self.laserShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'scan', LaserScan, queue_size=1)
            self.laserGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'scan', LaserScan, queue_size=1)
            self.laserGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'scan', LaserScan, queue_size=1)
            self.laserSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'scan', LaserScan, queue_size=1)
            self.laserSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'scan', LaserScan, queue_size=1)
            self.laserSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'scan', LaserScan, queue_size=1)

        self.laserSRDFrontPublisher_test = rospy.Publisher("~/pepper_navigation/front", LaserScan, queue_size=1)

    def __init__(self):
        NaoqiNode.__init__(self, 'nao_diagnostic_updater')

        # ROS initialization:
        self.connectNaoQi()

        # Read parameters:
        self.sleep = 1.0/rospy.get_param('~update_rate', 0.5)  # update rate in Hz
        self.warningThreshold = rospy.get_param('~warning_threshold', 68) # warning threshold for joint temperatures
        self.errorThreshold = rospy.get_param('~error_threshold', 74)     # error threshold for joint temperatures
        # check if NAOqi runs on the robot by checking whether the OS has aldebaran in its name
        self.runsOnRobot = "aldebaran" in os.uname()[2]   # if temperature device files cannot be opened, this flag will be set to False later.

        # Lists with interesting ALMemory keys
        self.jointNamesList = self.motionProxy.getJointNames('Body')    
        self.jointTempPathsList = []
        self.jointStiffPathsList = []    
        for i in range(0, len(self.jointNamesList)):
            self.jointTempPathsList.append("Device/SubDeviceList/%s/Temperature/Sensor/Value" % self.jointNamesList[i])
            self.jointStiffPathsList.append("Device/SubDeviceList/%s/Hardness/Actuator/Value" % self.jointNamesList[i])

        self.batteryNamesList = ["Percentage", "Status"]
        self.batteryPathsList = ["Device/SubDeviceList/Battery/Charge/Sensor/Value", 
                                 "Device/SubDeviceList/Battery/Charge/Sensor/Status",
                                 "Device/SubDeviceList/Battery/Current/Sensor/Value"]
        
        self.deviceInfoList = ["Device/DeviceList/ChestBoard/BodyId"]
        deviceInfoData = self.memProxy.getListData(self.deviceInfoList)
        if(len(deviceInfoData) > 1 and isinstance(deviceInfoData[1], list)):
            deviceInfoData = deviceInfoData[1]
        if(deviceInfoData[0] is None or deviceInfoData[0] == 0):
            # No device info -> running in a simulation
            self.isSimulator = True
            if(self.pip.startswith("127.") or self.pip == "localhost"):
                # Replace localhost with hostname of the machine
                f = open("/etc/hostname")
                self.hardwareID = "%s:%d"%(f.readline().rstrip(), self.pport)
                f.close()
            else:
                self.hardwareID = "%s:%d"%(self.pip, self.pport)            
        else:
            self.hardwareID = deviceInfoData[0].rstrip()
            self.isSimulator = False
            
        # init. messages:        
        self.diagnosticPub = rospy.Publisher("diagnostics", DiagnosticArray)

        rospy.loginfo("nao_diagnostic_updater initialized")

    def __init__( self ):

        #Initialisation
        NaoqiNode.__init__( self, self.NODE_NAME )
        if self.get_version() < distutils.version.LooseVersion('2.0'):
            rospy.logwarn("{} is only available on NaoQi version 2.0 or higher, your version is {}".format(self.NODE_NAME, self.get_version()))
            exit(1)

        #Proxy to interface with LEDs
        self.proxy = self.get_proxy( "ALAutonomousLife" )

        # Register ROS services
        self.disabled_srv = rospy.Service("~disabled", Empty, self.disabled )
        self.solitary_srv = rospy.Service("~solitary", Empty, self.solitary )
        self.interactive_srv = rospy.Service("~interactive", Empty, self.interactive )
        self.safeguard_srv = rospy.Service("~safeguard", Empty, self.safeguard )