CODEWITHSUNDEEP
pythonmultithreadingnao-robot
Stephen Martinez
Stephen Martinez

Reputation: 13

Trouble with threading and when to stop them

import time
import threading
import qi
import balldetection
import common
import cv2
import vision_definitions
import math 

lock = threading.Lock()

ball_detected = False
ip = "10.42.0.98"
port = 9559
session = qi.Session()
session.connect("tcp://" + ip + ":" + str(port))
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
video_service = session.service("ALVideoDevice")
speak_service = session.service("ALTextToSpeech")

def detect_ball_and_act():
    global ball_detected
    print("In detect_ball_and_act")
    videoClient = video_service.subscribeCamera("nao_opencv", 0, vision_definitions.kQVGA, vision_definitions.kBGRColorSpace, 5)
while not ball_detected:
    print("Not ball detected")
    try:
        print("In try")
        frame = common.readImage(video_service, videoClient)
        if frame is not None:
            print("in none")
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            img, (ret, x, y, radius, distance) = balldetection.detect_ball(hsv)
            if ret:
                print("Ball in sight at ({}, {}) with radius {} and distance {}".format(x, y, radius, distance))
                print("Performing action...")
                speak_service.say("Ball is in sight!")
                with lock:
                    ball_detected = True
            else:
                time.sleep(1.0)
    except Exception as e:
        print("An error occurred:", e)

def keep_walking_and_looking():
    while not ball_detected:
        print("Walking and looking around...")
        walk_and_look()

 def walk_and_look():
    print("In walk_and_look")
    motion_service.moveToward(0.5, 0, 0)
    time.sleep(3.0)
    motion_service.moveToward(0, 0, 0)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"],     [0.5, 1.0], 0.2)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [-0.5, 1.0], 0.2)
    time.sleep(1.0)
    motion_service.angleInterpolationWithSpeed(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.2)

walking_thread = threading.Thread(target=keep_walking_and_looking)
ball_detection_thread = threading.Thread(target=detect_ball_and_act)

motion_service.setStiffnesses("Body", 1.0)
posture_service.goToPosture("StandInit", 1.0)

walking_thread.start()
ball_detection_thread.start()

ball_detection_thread.join()
walking_thread.join()

The code does what I want but I want to know if there is a better way of handling this. This is for a Nao robot and his task for now is to walk around the room and look for a ball. As of now he does look for the ball and walks around the room but after he sees the ball, he keeps walking which is something I would like to stop. I know it's because the thread is still running and coming to a stop after he completes his walk but I want him to stop completely after seeing the ball since the next step is for him to walk up to the ball. It's supposed to be like a state machine.

Upvotes: 0

Views: 37

Answers (1)

Alexandre Mazel
Alexandre Mazel

Reputation: 2558

the naoqi API has been made to simplify multithreading. So to move and look, you could just for instance do a motion_service.post.angleInterpolationWithSpeed the head will move but your thread isn't blocked, so you can continue to do something at the same time.

This being said to stop your move abruptly when the robot sees the ball you can use a command of motion to stop, eg: motion_service.stopWalk or motion_service.stopMove. look also at the id returned by the post method.

idToKillTaskLater = motion_service.post.angleInterpolationWithSpeed

Don't hesitate to look at the NAOqi's documentation and some samples you can find in it...

Upvotes: 0

Related Questions