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.
