Question

这几天，我尝试使用Github项目Jumpstarter（https://github.com/aldebaran/robot-jumpstarter）中的pythonapp模板生成应用程序，以进行Pepper的本地化。我的基本想法是在生成的应用程序“ Lokalisierung”（德语本地化）中组合LandmarkDetector模块。 mainlandmarkdetection

您可以在此处阅读“ LandmarkDetector.py”，“ main.py”和“ MainLandmarkDetection.py”的完整代码：

“ LandmarkDetector.py”：

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

“”“示例：演示了一种通过以下方式定位机器人的方法 ALLandMarkDetection“”“

import qi
import time
import sys
import argparse
import math
import almath


class LandmarkDetector(object):

"""
We first instantiate a proxy to the ALLandMarkDetection module
Note that this module should be loaded on the robot's naoqi.
The module output its results in ALMemory in a variable
called "LandmarkDetected".
We then read this ALMemory value and check whether we get
interesting things.
After that we get the related position of the landmark compared to robot.
"""

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()

“ main.py”：

“”“显示如何将Python脚本制作为应用程序的示例。”“”

版本 =“ 0.0.8”

版权 =“版权2015，Aldebaran Robotics”   作者 ='YOURNAME'   电子邮件 ='YOUREMAIL@aldebaran.com'

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

class Activity(object):

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

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)

“ MainLandmarkDetection.py”：

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

“”“显示如何将Python脚本作为应用进行本地化的示例   具有ALLandMarkDetection“”“
的机器人
版本 =“ 0.0.8”

版权 =“版权2015，Aldebaran Robotics”

作者 ='YOURNAME'

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

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

class Activity(object):

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

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()

这是在cmd.exe中的工作方式：

bugs_for_mainlandmarkdetection02 而且我在程序快结束时对第157行中的“ landmark_detector = Activity()”参数提出了疑问，因为我应该通过图像中的错误。在阅读完Stackoverflow Python: TypeError: __init__() takes exactly 2 arguments (1 given)对类似问题的答案后，我仍然感到困惑。我认为应该赋予“ qiapp”一个值，但是什么值呢？

最好的问候，弗雷德里克

Answer 1

实际上，当您致电时

stk.runner.run_activity(Activity)

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

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

还要注意，除了调用sys.stop（）之外，您还可以调用self.stop（）或self.qiapp.stop（）（就让on_stop中的清除代码被调用而言，这更干净一点）。，如果您需要取消订阅等）

有关stk.runner的一些文档，请参见here。

还请注意，不要这样做

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

您可以直接做

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

...（在我看来），这使得更容易准确地查看正在执行的操作（并减少了变量的数量）。

适用于机器人的Python：如何生成用于Pepper机器人本地化的应用

“ LandmarkDetector.py”：

“ main.py”：

“ MainLandmarkDetection.py”：

这是在cmd.exe中的工作方式：

1 个答案: