Python for Robotics：如何为Pepper robot的本地化生成应用程序

最近，我尝试使用Github项目Jumpstarter（）中的pythonapp模板生成一个应用程序，以对Pepper进行本地化。我的基本想法是在生成的应用程序“Lokalisierung”（德语本地化）中结合LandmarkDetector模块。

您可以在此处阅读“LandmarkDetector.py”、“main.py”和“MainLandmarkDetection.py”的全部代码：

“LandmarkDetector.py”： “”“示例：演示一种使用 阿兰马克

“main.py”： “”“演示如何将Python脚本制作为应用程序的示例。”“”

版本=“0.0.8”

版权所有=“2015年版权所有，Aldebaran机器人” 作者=“你的名字” 电子邮件='YOUREMAIL@aldebaran.com"

“一个示例独立应用程序，演示了简单的Python用法” APP_ID=“com.aldebaran.lokaliseerung”

“MainLandmarkDetection.py”： “”“一个示例，显示如何将Python脚本制作为要本地化的应用程序 具有AllandMarket功能的机器人

版本=“0.0.8”

版权所有=“2015年版权所有，Aldebaran机器人”

作者=“你的名字”

电子邮件='YOUREMAIL@aldebaran.com"

“一个示例独立应用程序，演示了简单的Python用法” APP_ID=“com.aldebaran.lokaliseerung”

这就是它在cmd.exe中的工作方式： 由于图像中的错误，我对程序末尾第157行中的参数“

landmark\u detector=Activity（）

”有一个问题，我应该通过它。在阅读了Stackoverflow对类似问题的回答后，我仍然感到困惑。我认为这应该是一个给“qiapp”的值，但是什么值呢

致以最良好的祝愿，

---

弗雷德里克实际上，当你打电话时

stk.runner.run_activity(Activity)

。。。它将为您实例化该活动对象，使用正确的参数，您不需要在MainLandmarkDetector.py中使用landmark_detector=activity（）等

如果此对象有一个名为on_start的方法，则在一切就绪后将调用该方法（因此您可能只需要将run（）方法重命名为on_start（）

还要注意的是，不调用sys.stop（），只需调用self.stop（）或self.qiapp.stop（）

有关stk.runner的一些文档，请参阅

还要注意的是，不要这样做

self.motion_service = session.service("ALMotion")
(...)
transform = self.motion_service.getTransform(self.currentCamera, 2, True)

你可以直接做

transform = self.s.ALMotion.getTransform(self.currentCamera, 2, True)

---

…这（在我看来）使我们更容易看到正在做的事情（并减少了变量的数量）。

谢谢，这真的很有帮助！如果我需要可以在cmd.exe下输出的输出也可以在Choregraph Log viewer窗口中看到呢？我需要编辑“Run app Executive”吗手动添加一些输出值，或者可以在模板的帮助下自动完成？（我已经在“Run app executable”python框中更改了相应的可执行文件\u id和可执行文件\u名称）我不确定如何在Choregraph中获取这些日志，在NAOqi 2.1中可能不可能，您必须尝试一些事情（可能它们已经在那里了？否则请签入/var/log/naoqi，但我不认为你会把它们带到那里）好的。这里是NAOqi2.5。你是说机器人上的日志，我应该在编舞中以“连接高级文件传输…”的方式获得这些日志吗？在路径“/var/log/”下只有一个名为“rabbitmaq”的文件夹……如果您使用NAOqi 2.5 ssh连接到robot上（检查“lsb_release-a”的robot上的版本），您应该在/var/log/NAOqi中找到日志。

def __init__(self, app):
    """
    Initialisation of qi framework and event detection.
    """
    super(LandmarkDetector, self).__init__()

    app.start()
    session = app.session
    # Get the service ALMemory.
    self.memory = session.service("ALMemory")
    # Connect the event callback.

    # Get the services ALMotion & ALRobotPosture.

    self.motion_service = session.service("ALMotion")
    self.posture_service = session.service("ALRobotPosture")

    self.subscriber = self.memory.subscriber("LandmarkDetected")
    print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
    self.subscriber.signal.connect(self.on_landmark_detected)
    print "self.subscriber.signal.connect(self.on_landmark_detected)"
    # Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
    self.tts = session.service("ALTextToSpeech")
    self.landmark_detection = session.service("ALLandMarkDetection")
  #  print "self.landmark_detection" is repr(self.landmark_detection)
    self.motion_service = session.service("ALMotion")
    self.landmark_detection.subscribe("LandmarkDetector", 500, 0.0 )
    print "self.landmark_detection.subscribe(LandmarkDetector, 500, 0.0 )"
    self.got_landmark = False
    # Set here the size of the landmark in meters.
    self.landmarkTheoreticalSize = 0.06 #in meters 0  #.05 or 0.06?
    # Set here the current camera ("CameraTop" or "CameraBottom").
    self.currentCamera = "CameraTop"

def on_landmark_detected(self, markData):
    """
    Callback for event LandmarkDetected.
    """
    while markData == [] :  # empty value when the landmark disappears
        self.got_landmark = False
        self.motion_service.moveTo(0, 0, 0.1 * math.pi)

    if not self.got_landmark:  # only speak the first time a landmark appears
        self.got_landmark = True

#stop.motion_service.moveTo

        print "Ich sehe eine Landmarke! "
        self.tts.say("Ich sehe eine Landmarke! ")

        # Retrieve landmark center position in radians.
        wzCamera = markData[1][0][0][1]
        wyCamera = markData[1][0][0][2]

        # Retrieve landmark angular size in radians.
        angularSize = markData[1][0][0][3]

        # Compute distance to landmark.
        distanceFromCameraToLandmark = self.landmarkTheoreticalSize / ( 2 * math.tan( angularSize / 2))

        # Get current camera position in NAO space.
        transform = self.motion_service.getTransform(self.currentCamera, 2, True)
        transformList = almath.vectorFloat(transform)
        robotToCamera = almath.Transform(transformList)

        # Compute the rotation to point towards the landmark.
        cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)

        # Compute the translation to reach the landmark.
        cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)

        # Combine all transformations to get the landmark position in NAO space.
        robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform

#    robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
#        print "robotTurnAroundAngle = ", robotTurnAroundAngle

        print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
        print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
        print "z " + str(robotToLandmark.r3_c4) + " (in meters)"

def run(self):
    """
    Loop on, wait for events until manual interruption.
    """

    # Wake up robot
    self.motion_service.wakeUp()

    # Send robot to Pose Init
    self.posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = self.motion_service.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    #   initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))

    # Make the robot move
    for i in range(1, 12, 1):
        self.motion_service.moveTo(0, 0, 0.1 * math.pi)
        print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"

    print "Starting LandmarkDetector"
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print "Interrupted by user, stopping LandmarkDetector"
        self.landmark_detection.unsubscribe("LandmarkDetector")
        #stop
        sys.exit(0)


if __name__ == "__main__":


    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="10.0.0.10",
                    help="Robot IP address. On robot or Local Naoqi: use 
'10.0.0.10'.")
    parser.add_argument("--port", type=int, default=9559,
                    help="Naoqi port number")

    args = parser.parse_args()
    try:
        # Initialize qi framework.
        connection_url = "tcp://" + args.ip + ":" + str(args.port)
        app = qi.Application(["LandmarkDetector", "--qi-url=" + connection_url])
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)
    landmark_detector = LandmarkDetector(app)
    landmark_detector.run()

import stk.runner
import stk.events
import stk.services
import stk.logging

class Activity(object):

def __init__(self, qiapp):
    self.qiapp = qiapp
    self.events = stk.events.EventHelper(qiapp.session)
    self.s = stk.services.ServiceCache(qiapp.session)
    self.logger = stk.logging.get_logger(qiapp.session, self.APP_ID)

def on_touched(self, *args):
    "Callback for tablet touched."
    if args:
        self.events.disconnect("ALTabletService.onTouchDown")
        self.logger.info("Tablet touched: " + str(args))
        self.s.ALTextToSpeech.say("Yay!")
        self.stop()

def on_start(self):
    "Ask to be touched, waits, and exits."
    # Two ways of waiting for events
    # 1) block until it's called

    self.s.ALTextToSpeech.say("Touch my forehead.")
    self.logger.warning("Listening for touch...")
    while not self.events.wait_for("FrontTactilTouched"):
        pass

    # 2) explicitly connect a callback
    if self.s.ALTabletService:
        self.events.connect("ALTabletService.onTouchDown", self.on_touched)
        self.s.ALTextToSpeech.say("okay, now touch my tablet.")
        # (this allows to simltaneously speak and watch an event)
    else:
        self.s.ALTextToSpeech.say("touch my tablet ... oh. " + \
            "I don't haave one.")
        self.stop()

def stop(self):
    "Standard way of stopping the application."
    self.qiapp.stop()

def on_stop(self):
    "Cleanup"
    self.logger.info("Application finished.")
    self.events.clear()

if __name__ == "__main__":


    stk.runner.run_activity(Activity)

#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

import stk.runner
import stk.events
import stk.services
import stk.logging
import time
import sys
import math
import almath

class Activity(object):

def __init__(self, qiapp):

    self.qiapp = qiapp
    self.events = stk.events.EventHelper(qiapp.session)
    self.s = stk.services.ServiceCache(qiapp.session)
    self.logger = stk.logging.get_logger(qiapp.session, self.APP_ID)
    self.qiapp.start()
    session = qiapp.session
    # Get the service ALMemory.
    self.memory = session.service("ALMemory")
    # Connect the event callback.

    # Get the services ALMotion & ALRobotPosture.

    self.motion_service = session.service("ALMotion")
    self.posture_service = session.service("ALRobotPosture")

    self.subscriber = self.memory.subscriber("LandmarkDetected")
    print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
    self.subscriber.signal.connect(self.on_landmark_detected)
    print "self.subscriber.signal.connect(self.on_landmark_detected)"
    # Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
    self.tts = session.service("ALTextToSpeech")
    self.landmark_detection = session.service("ALLandMarkDetection")
    #  print "self.landmark_detection" is repr(self.landmark_detection)
    self.motion_service = session.service("ALMotion")
    self.landmark_detection.subscribe("Activity", 500, 0.0)
    print "self.landmark_detection.subscribe(Activity, 500, 0.0 )"
    self.got_landmark = False
    # Set here the size of the landmark in meters.
    self.landmarkTheoreticalSize = 0.06  # in meters 0  #.05 or 0.06?
    # Set here the current camera ("CameraTop" or "CameraBottom").
    self.currentCamera = "CameraTop"

def on_landmark_detected(self, markData):
    """
    Callback for event LandmarkDetected.
    """
    while markData == []:  # empty value when the landmark disappears
        self.got_landmark = False
    #           self.motion_service.moveTo(0, 0, 0.1 * math.pi)

    if not self.got_landmark:  # only speak the first time a landmark appears
        self.got_landmark = True

        # stop.motion_service.moveTo

        print "Ich sehe eine Landmarke! "
        #          self.tts.say("Ich sehe eine Landmarke! ")

        # Retrieve landmark center position in radians.
        wzCamera = markData[1][0][0][1]
        wyCamera = markData[1][0][0][2]

        # Retrieve landmark angular size in radians.
        angularSize = markData[1][0][0][3]

        # Compute distance to landmark.
        distanceFromCameraToLandmark = self.landmarkTheoreticalSize / (2 * math.tan(angularSize / 2))

        # Get current camera position in NAO space.
        transform = self.motion_service.getTransform(self.currentCamera, 2, True)
        transformList = almath.vectorFloat(transform)
        robotToCamera = almath.Transform(transformList)

        # Compute the rotation to point towards the landmark.
        cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)

        # Compute the translation to reach the landmark.
        cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)

        # Combine all transformations to get the landmark position in NAO space.
        robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform * cameraToLandmarkTranslationTransform

        #    robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
        #        print "robotTurnAroundAngle = ", robotTurnAroundAngle

        print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
        print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
        print "z " + str(robotToLandmark.r3_c4) + " (in meters)"

def run(self):
    """
    Loop on, wait for events until manual interruption.
    """

    # Wake up robot
    self.motion_service.wakeUp()

    # Send robot to Pose Init
    self.posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = self.motion_service.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    #   initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))

    # Make the robot move
    for i in range(1, 20, 1):
        self.motion_service.moveTo(0, 0, 0.1 * math.pi)
        print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"

    print "Starting Activity"
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print "Interrupted by user, stopping Activity"
        self.landmark_detection.unsubscribe("Activity")
        # stop
        sys.exit(0)

    def stop(self):
        "Standard way of stopping the application."
        self.qiapp.stop()

    def on_stop(self):
        "Cleanup"
        self.logger.info("Application finished.")
        self.events.clear()

if __name__ == "__main__":



    stk.runner.run_activity(Activity)

    landmark_detector = Activity()

    landmark_detector.run()

stk.runner.run_activity(Activity)

self.motion_service = session.service("ALMotion")
(...)
transform = self.motion_service.getTransform(self.currentCamera, 2, True)

transform = self.s.ALMotion.getTransform(self.currentCamera, 2, True)