🕵️‍♂️Misty follow human

Misty can track and follow you!

You can use this code to track and follow all Misty's known objects. In this example, Misty will follow just object 0 - "person".

from mistyPy.Robot import Robot
from mistyPy.Events import Events
import time
import math

misty = Robot("YOUR_ROBOT_IP_ADDRESS")
misty.change_led(0, 255, 0)
misty.move_head(0, 0, 0)

#modes
turn_in_place = True
follow_human = True

#constants
yaw_left = 81.36
yaw_right = -85.37
pitch_up = -40.10
pitch_down = 26.92

#variables
curr_head_pitch = 0
curr_head_yaw = 0

#Event handler for getting the current head position
def curr_position(data):
    global curr_head_pitch, curr_head_yaw
    if data["message"]["sensorId"] == "ahp":
        curr_head_pitch = data["message"]["value"]
        print(curr_head_pitch)
    if data["message"]["sensorId"] == "ahy":
        curr_head_yaw = data["message"]["value"]
        print(curr_head_yaw)

def get_pos():
    misty.register_event(event_name="get_curr_position", event_type= Events.ActuatorPosition, keep_alive= True, callback_function=curr_position)
    time.sleep(0.25)
    misty.unregister_event(event_name="get_curr_position")

# Event handler for person detection
def person_detection(data):
    print(data)
    if data["message"]["confidence"] >= 0.6:
        width_of_human = data["message"]["imageLocationRight"] - data["message"]["imageLocationLeft"]
        x_error = (160.0 - (data["message"]["imageLocationLeft"] + data["message"]["imageLocationRight"]) / 2.0) / 160.0
        # Use this for non-human tracking
        # y_error = (160.0 - ((data["message"]["imageLocationTop"] + data["message"]["imageLocationBottom"]) / 2.0)) / 160.0
        
        #Use this for human tracking
        y_error = (160.0 - 0.8 * data["message"]["imageLocationTop"] - 0.2 * data["message"]["imageLocationBottom"]) / 160.0

        threshold = max((0.3 if turn_in_place or follow_human else 0.2), (321.0 - width_of_human) / 1000.0)
        damper_gain = 5.0 if turn_in_place or follow_human else 7.0

        get_pos()
        actuate_to_yaw = curr_head_yaw + x_error * ((yaw_left - yaw_right) / damper_gain) if abs(x_error) > threshold else None
        actuate_to_pitch = curr_head_pitch - y_error * ((pitch_down - pitch_up) / 3.0) if abs(y_error) > threshold else None

        linear_velocity = 0
        angular_velocity = 0

        if actuate_to_yaw and abs(actuate_to_yaw) > 15 and (turn_in_place or follow_human):
            angular_velocity = math.copysign(min(abs(actuate_to_yaw) * 0.6, 25), actuate_to_yaw)

        if angular_velocity != 0:
            if math.copysign(1, actuate_to_yaw - curr_head_yaw) == math.copysign(1, angular_velocity):
                if abs(actuate_to_yaw) > 40:
                    actuate_to_yaw /= 1.5
            else:
                actuate_to_yaw = 0
                angular_velocity = 0
                if not follow_human:
                    misty.stop()

        if follow_human:
            if angular_velocity == 0:
                linear_velocity = (130 - width_of_human) * 0.5
                linear_velocity = min(abs(linear_velocity), 20) * math.copysign(1, linear_velocity)
                linear_velocity = linear_velocity if abs(linear_velocity) > 5 else 0
                misty.change_led(0, 255, 255)

        misty.move_head(actuate_to_pitch, None, actuate_to_yaw)
        if turn_in_place or follow_human:
            misty.drive(linear_velocity, angular_velocity)

misty.start_object_detector(0.5, 0, 15)
misty.register_event(event_name="personDetection", event_type= Events.ObjectDetection, callback_function=person_detection, debounce=500, keep_alive=True)

misty.keep_alive()

Modes

There are three different modes that you can use in this Python code (Misty's head will always move):

Mode 1: No driving ("turnInPlace" : false, "followHuman" : false)
Mode 2: Allow turning in place, but not driving forward/backward ("turnInPlace" : true, "followHuman" : false)
Mode 3: Allow full driving ("turnInPlace" : true, "followHuman" : true)

Constants

Those constants are used to calibrate the maximum range of movement of Misty's head. Those should be equal for all Misty's but it would be preferred to check them before you run your code to have better accuracy.

To do it you can use this code:

from mistyPy.Robot import Robot
from mistyPy.Events import Events
import time

misty = Robot("YOUR_ROBOT_IP_ADDRESS")
misty.change_led(0, 255, 0)
misty.move_head(0, 0, 0)

#constants
yaw_left = 0
yaw_right = 0
pitch_up = 0
pitch_down = 0

print(------)
#get current position functions
def curr_position(data):
    global ------ 
    if data["message"]["sensorId"] == "ahp":
        ------ = data["message"]["value"]
        time.sleep(0.5)
        print(------)

def get_pos():
    misty.move_head(90, 0, 0)
    time.sleep(5)
    misty.register_event(event_name="get_curr_position", event_type= Events.ActuatorPosition, keep_alive= True, callback_function=curr_position)
    time.sleep(0.25)
    misty.unregister_event(event_name="get_curr_position")

get_pos()

Replace in the marked space (------) the name of the position that you want to check and adjust the move_head parameters and the type of data according to the following table

Position	move_head	SensorID
yaw_left	move_head(0, 0, 90)	"ahy"
yaw_right	move_head(0, 0, -90)	"ahy"
pitch_up	move_head(-90, 0, 0)	"ahp"
pitch_down	move_head(90, 0, 0)	"ahp"

For example, if you want to check the position of the yaw left your code should look like this one:

from mistyPy.Robot import Robot
from mistyPy.Events import Events
import time

misty = Robot("YOUR_ROBOT_IP_ADDRESS")
misty.change_led(0, 255, 0)
misty.move_head(0, 0, 0)

#constants
yaw_left = 0
yaw_right = 0
pitch_up = 0
pitch_down = 0

print(yaw_left)
#get current position functions
def curr_position(data):
    global yaw_left 
    if data["message"]["sensorId"] == "ahy":
        yaw_left = data["message"]["value"]
        time.sleep(0.5)
        print(yaw_left)

def get_pos():
    misty.move_head(0, 0, 90)
    time.sleep(5)#give Misty enough time to achieve the position
    misty.register_event(event_name="get_curr_position", event_type= Events.ActuatorPosition, keep_alive= True, callback_function=curr_position)
    time.sleep(0.25)
    misty.unregister_event(event_name="get_curr_position")

get_pos()

Then you can replace this number in the constants of the main code.

The variables will contain Misty's current head position and the following functions are used:

• Record the position and get it as a number memorized in the variables and follow the human.

• Detect and follow the person.

The math in the code is to better handle the person's position and adapt Misty's movements.