def move_miro():
#publishing topics
pub = rospy.Publisher('/miro/rob01/platform/control', platform_control, queue_size=10)
pub2 = rospy.Publisher('/miro/rob01/core/config', core_config, queue_size=10)
#initiation
rospy.init_node('move_miro', anonymous=True)
rate = rospy.Rate(10)
q = platform_control()
q2 = core_config()
count = 0
#core config flag setup
q2.msg_flags = core_config.FLAG_UPDATE_SIGNALS
q2.P2U_W_body_signals = miro.MIRO_P2U_W_BODY_ENABLE
pub2.publish(q2)
while not rospy.is_shutdown():
#q.body_vel.linear.x = 240
#q.body_vel.linear.y = 240
#q.body_vel.angular.z = 0
q.body_config_speed = [0, -1,-1,0]
q.body_config = [0, -3.14, 0, 0]
pub.publish(q)
rospy.loginfo(count)
rate.sleep()
count +=1
def shutdow_hook(self):
"""Stops the core behavior and saves the file, when the program is stopped"""
# Stop core behavior
c = core_config()
c.msg_flags = c.FLAG_UPDATE_SIGNALS
self.pub_core_config.publish(c)
# Set MiRo's joints to reset
q = platform_control()
q.body_config_speed = [-1, -1, -1, -1]
self.platform_control_pub.publish(q)
# Save only if a full week has passed
if self.end == True:
# Try to save the file
try:
with open(self.monitor_file, "w") as filehandle:
# Change the indentation for a nice humanreadable format
change_indent(self.monitor_list)
print(self.monitor_list)
# Save the file
filehandle.write(
json.dumps(self.monitor_list,
cls=NoIndentEncoder,
indent=4))
print("file saved")
except IOError:
print("error at saving")
def loop(self):
rate = rospy.Rate(1) #1hz
while not rospy.is_shutdown():
rate.sleep()
if self.test:
c = core_config()
c.msg_flags = c.FLAG_UPDATE_SIGNALS
print c
self.pub_core_config.publish(c)
def __init__(self, topic_root, follow_type):
self.pub_platform_control = rospy.Publisher(topic_root + "/platform/control", platform_control, queue_size=1)
if follow_type == "mics":
# listen to microphones to follow a sound
self.mics_sub = rospy.Subscriber(topic_root + "/platform/mics", platform_mics, self.callback_mics_follow, queue_size=1)
elif follow_type == "core":
# activate spatial behavior
self.core_config_pub = rospy.Publisher(topic_root + "/core/config", core_config, queue_size=0)
# some publisher that does not hurt
self.core_control_pub = rospy.Publisher(topic_root + "/core/control", core_control, queue_size=1)
time.sleep(1)
self.c = core_config()
self.c.P2S_W_signals = self.c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_ENABLE
self.c.msg_flags = self.c.FLAG_UPDATE_SIGNALS
self.core_config_pub.publish(self.c)
# listen to it (core audio event)
self.core_sub = rospy.Subscriber(topic_root + "/core/state", core_state, self.callback_core_follow)
else:
print("Enter a valid mode: mics|core")
rospy.signal_shutdown("Enter a valid mode: mics|core")
def config_send(self):
c = core_config()
c.P2B_W_signals = c.P2B_W_signals | miro.MIRO_P2B_W_BRANCH_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ADJUST_RTC
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_VALENCE_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_AROUSAL_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE_SLEEP
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_CLOCK
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_WAKEFULNESS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_TOUCH
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_LIGHT
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SOUND
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_ACCEL
#c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SLEEP_BLOCKED
#c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_RANDOMIZE_VALENCE
#c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FAST_SLEEP_DYNAMICS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_LIGHT
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_TAIL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EYELIDS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EARS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_VOCAL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_BODY
#c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_PING
#c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_NO_PIRATE_NOISES
#c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_DO_PIRATE_NOISES
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_ENABLE
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DEBUG
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_FORCE_MULL
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_RANDOMIZE_ORIENT
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DISABLE_HALT
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_MODULATE_BY_SONAR
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_ENABLE
#c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_RESET_KC_INTEGRATORS
#c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH
#c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_MOTION
#c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_TRANSLATION
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_INTO_SONAR
#c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_POS_CONTROL
c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_CLIFF_REFLEX
c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_ENABLE
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_AUDIO
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_VIDEO
c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_PRIORITY
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_OTHER
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_REAFF_COMPROMISE
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_SUPPRESS
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_COMPROMISE
#c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_TEST_PATTERN
#c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_TEST_ALARM
#c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_NO_I2C_BUSY_ALARM
c.msg_flags = c.FLAG_UPDATE_SIGNALS
self.pub_core_config.publish(c)
print("config sent")
# bridge/config
q = bridge_config()
q.flags = miro.MIRO_BRIDGE_UPDATE
self.pub_bridge_config.publish(q)
print("bridge/config sent")
'''
def __init__(self):
# report
print("initialising...")
print(sys.version)
# default data
self.platform_sensors = None
self.platform_state = None
self.platform_mics = None
self.core_state = None
# no arguments gives usage
if len(sys.argv) == 1:
usage()
# options
self.robot_name = ""
self.test = False
# handle args
for arg in sys.argv[1:]:
f = arg.find('=')
if f == -1:
key = arg
val = ""
else:
key = arg[:f]
val = arg[f + 1:]
if key == "robot":
self.robot_name = val
elif key == "test":
self.test = True if val == "True" else False
else:
error("argument not recognised \"" + arg + "\"")
# check we got at least one
if len(self.robot_name) == 0:
error("argument \"robot\" must be specified")
# pattern
self.count = 0
self.z_bak = -1
self.body_vel = None
self.turn = False
# set inactive
self.active = False
# topic root
topic_root = "/miro/" + self.robot_name
print "topic_root", topic_root
# publish
self.pub_platform_control = rospy.Publisher(topic_root + "/platform/control",
platform_control, queue_size=0)
self.pub_core_control = rospy.Publisher(topic_root + "/core/control",
core_control, queue_size=0)
self.pub_core_config = rospy.Publisher(topic_root + "/core/config",
core_config, queue_size=0)
# self.pub_bridge_config = rospy.Publisher(topic_root + "/bridge/config",
# bridge_config, queue_size=0)
# self.pub_bridge_stream = rospy.Publisher(topic_root + "/bridge/stream",
# bridge_stream, queue_size=0)
# self.pub_platform_config = rospy.Publisher(topic_root + "/platform/config",
# platform_config, queue_size=0)
# subscribe
if self.test:
rospy.Subscriber(topic_root + "/platform/sensors",
platform_sensors, self.callback_platform_sensors)
c = core_config()
c.P2B_W_signals = c.P2B_W_signals | miro.MIRO_P2B_W_BRANCH_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_DO_PIRATE_NOISES
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE_SLEEP
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_ENABLE
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_FORCE_MULL
#c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_RANDOMIZE_ORIENT
#c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_TEST_ALARM
c.msg_flags = c.FLAG_UPDATE_SIGNALS
print c
self.pub_core_config.publish(c)
time.sleep(1)
else:
self.sub_sensors = rospy.Subscriber(topic_root + "/platform/sensors",
platform_sensors, self.callback_move_until_obstacle)
rospy.Subscriber(topic_root + "/platform/state",
platform_state, self.callback_turn_180)
# self.sub_state = rospy.Subscriber(topic_root + "/platform/state",
# platform_state, self.callback_platform_state)
# self.sub_mics = rospy.Subscriber(topic_root + "/platform/mics",
# platform_mics, self.callback_platform_mics)
# self.sub_core_state = rospy.Subscriber(topic_root + "/core/state",
# core_state, self.callback_core_state)
# set active
self.active = True
def __init__(self):
"""Prepares MiRo
E.g. setting all neccessary starting conditions and read the old log file
"""
# First register the node
rospy.init_node("miro_ros_client_py", anonymous=True)
# Get all parameters from the launch file
self.robot_name = rospy.get_param("~robot_name")
self.demo_mode = rospy.get_param("~demo_mode")
self.start_wait = rospy.get_param("~start_wait")
self.interval_time = rospy.get_param("~interval_time")
self.days = rospy.get_param("~days")
self.hours_per_day = rospy.get_param("~intervals_per_day")
self.monitor_file = rospy.get_param("~monitor_file") + "_" + str(self.days) + "_" + str(self.hours_per_day) + "_" + str(self.interval_time) + ".log"
# Prepare the dictionaries for the log file
self.event_template = {"happiness": [], "sadness": [], "blink": [], "surprise": [],
"fear": [], "anger": [], "disgust": [], "contempt": [], "head_touch": [], "body_touch": [], "face_on_floor": []}
self.hour_template = {"hour": [], "predicted_pattern": [], "observed_pattern": [], "events": copy.deepcopy(self.event_template)}
self.hours_template = [copy.deepcopy(self.hour_template) for _ in range(self.hours_per_day)]
self.day_template = {"day": [], "day_group": [], "hours": self.hours_template}
# Start at day 0 and hour 0
self.current_day = 0
self.current_hour = 0
# The Program is not yet at the end
self.end = False
# The Program is not active yet
self.active = False
# Add a shutdown hook, to savely close the program
rospy.on_shutdown(self.shutdow_hook)
# try to load the file
try:
with open(self.monitor_file, "r") as filehandle:
print("load the file")
print(self.monitor_file)
self.monitor_list = json.load(filehandle)
# Increment the number of runs by one
self.col_id = self.monitor_list[0] + 1
self.monitor_list[0] = self.col_id
# if the file does not exist, create a new file
except IOError:
print("create a new file")
self.col_id = 0
self.monitor_list = [self.col_id, [copy.deepcopy(self.day_template) for _ in range(self.days)]]
# Set all counters to 0
self.happiness = 0
self.sadness = 0
self.blink = 0
self.surprise = 0
self.fear = 0
self.anger = 0
self.disgust = 0
self.contempt = 0
self.face_on_floor = 0
self.head_touch = 0
self.body_touch = 0
# Prepare miro's topic
topic_root = "/miro/" + self.robot_name
# Set up all publishers, we only use core/control and core/config
self.pub_core_control = rospy.Publisher(topic_root + "/core/control", core_control, queue_size=0)
self.pub_core_config = rospy.Publisher(topic_root + "/core/config", core_config, queue_size=0)
# Wait the time specified in launch file. This is necessary, because the publishers may not be ready immediately
rospy.sleep(self.start_wait)
# After the waiting time, set the starting time
self.start_time = rospy.Time.now().to_sec()
# Set the dict to map from patterns to states
self.pattern_to_state_dict = {"nothing": "Sleep", "face_on_floor": "Angry_disturbed", "happy_face": "Happy_active",
"body_touch": "Happy_active", "head_touch": "Happy_calm", "sad_face": "Angry_calm", "neutral": "Neutral"}
# Set the dict to map from states to /core/control messages
self.state_dict = {"Neutral": [0.5, 0.5, 0.5, 0.5, 1.0, 0.0], "Angry_disturbed": [0.0, 1.0, 0.0, 1.0, 1.0, 0.0],
"Angry_calm": [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], "Happy_active": [1.0, 1.0, 1.0, 1.0, 1.0, 0.0],
"Happy_calm": [1.0, 0.0, 1.0, 0.0, 1.0, 0.0], "Sleep": [0.5, 0.0, 0.5, 0.0, 0.0, 1.0]}
# Predict the pattern for the first time interval
self.predict_next_pattern()
# If the flag to use miro's demo behavior was set, activate the demo behavior
if self.demo_mode == True:
c = core_config()
c.P2B_W_signals = c.P2B_W_signals | miro.MIRO_P2B_W_BRANCH_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ADJUST_RTC ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_VALENCE_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_AROUSAL_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE_SLEEP ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_CLOCK ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_WAKEFULNESS ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_TOUCH
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_LIGHT
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SOUND
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_ACCEL
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SLEEP_BLOCKED
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_RANDOMIZE_VALENCE
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FAST_SLEEP_DYNAMICS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_LIGHT
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_TAIL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EYELIDS
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EARS ## ?
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_VOCAL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_BODY
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_DEBUG_SONAR
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_NO_PIRATE_NOISES
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_DO_PIRATE_NOISES
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_ENABLE
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DEBUG
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_FORCE_MULL
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_RANDOMIZE_ORIENT
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DISABLE_HALT
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_MODULATE_BY_SONAR
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_ENABLE
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_RESET_KC_INTEGRATORS
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH ## no movement
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_MOTION
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_TRANSLATION
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_INTO_SONAR
# c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_POS_CONTROL
c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_CLIFF_REFLEX
c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_ENABLE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_AUDIO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_VIDEO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_PRIORITY
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_OTHER
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_REAFF_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_SUPPRESS
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_TEST_PATTERN
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_TEST_ALARM
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_NO_I2C_BUSY_ALARM
c.msg_flags = c.FLAG_UPDATE_SIGNALS
# Publish the core/control message to activate miro's core behavior
self.pub_core_config.publish(c)
# Add all subscribers
# Platform state
self.platform_state = rospy.Subscriber(topic_root + "/platform/state", platform_state, self.callback_state, queue_size=1)
# Face on the florr
self.face_sub = rospy.Subscriber("/faceCoord", Int32MultiArray, self.callback_faces)
# Face emotions
self.face_sub = rospy.Subscriber("/faces", Faces, self.callback_emotions)
# Platform sensors (used for Touch)
self.sub_platform_sensors = rospy.Subscriber(topic_root + "/platform/sensors", platform_sensors, self.callback_sensors)
def location_miro():
# get relative destination
# param_name = rospy.search_param('virtual_x')
# x_dist = rospy.get_param(param_name)
# param_name2 = rospy.search_param('virtual_y')
# y_dist = rospy.get_param(param_name2)
param_name = rospy.search_param('turn_dir')
kak = rospy.get_param(param_name)
# robot target
robot = '/miro/rob01'
# initialise ROS node
rospy.init_node('location_miro', anonymous=True)
# topic publishers
pub = rospy.Publisher(robot + '/platform/control', platform_control, queue_size=10)
pub2 = rospy.Publisher(robot + '/core/config', core_config, queue_size=10)
# topic subscribers
sub = rospy.Subscriber(robot + '/platform/state', platform_state, callback_state)
# intialise rate object
rate = rospy.Rate(10)
# sleep to let the ROS node start up, or our config message
# will not make it through
time.sleep(1)
# configure core to enable BODY module
q = core_config()
q.msg_flags = core_config.FLAG_UPDATE_SIGNALS
q.P2U_W_body_signals = miro.MIRO_P2U_W_BODY_ENABLE
pub2.publish(q)
# create control message
q = platform_control()
#q.body_move.x = x_dist
#q.body_move.y = y_dist
if kak:
q.body_move.x = 600 #in mm, positive to go forward
q.body_move.y = -800 #negative to go to the right, positive to the left
q.body_move.theta = 0
rospy.set_param('turn_dir',False)
else:
q.body_move.x = 600 #in mm, positive to go forward
q.body_move.y = 800 #negative to go to the right, positive to the left
q.body_move.theta = 0
rospy.set_param('turn_dir',True)
# close eyes and hope for the best!
q.eyelid_closure = 1
# main loop
count = 1
while not rospy.is_shutdown():
# add if condition here to check for state_avoid parameter
# configure move flags
if count >= 2:
q.body_move_flags = miro.MIRO_BODY_MOVE_CONTINUE
if count == 2:
q.body_move_flags |= miro.MIRO_BODY_MOVE_START
# publish control message
pub.publish(q)
# postamble
rospy.loginfo("@" + str(count) + ", move_underway=" + str(move_underway))
rate.sleep()
count += 1
print count
# exit
if move_complete:
#global counter
rospy.set_param('state_straight', True)
rospy.set_param('state_avoid', False)
#counter=2
break;
def __init__(self):
"""Prepares MiRo
E.g. setting all neccessary starting conditions and read the old log file
"""
# Get all parameters from the launch file
self.robot_name = rospy.get_param("~robot_name")
self.demo_mode = rospy.get_param("~demo_mode")
self.demo_move = rospy.get_param("~demo_move")
self.start_wait = rospy.get_param("~start_wait")
self.interval_time = rospy.get_param("~interval_time")
self.days = rospy.get_param("~days")
self.hours_per_day = rospy.get_param("~intervals_per_day")
self.monitor_emotions = rospy.get_param("~monitor_emotions")
self.monitor_file = "%s_%s_%s_%s_%s.log" % (
rospy.get_param("~monitor_file"), str(
self.days), str(self.hours_per_day), str(
self.interval_time), str(self.monitor_emotions))
self.min_part_conf = rospy.get_param("~min_part_conf")
self.monitor_emotions = rospy.get_param("~monitor_emotions")
# Prepare the dictionaries for the log file
# There are 5 different categories of things MiRo monitors + one uncategorized category
self.toy_template = {
"ball_b": [],
"ball_y": [],
"ball_r": [],
"ball_g": [],
"ball_rb": [],
"screwdriver": [],
"pen": [],
"ribena": [],
"moto5": [],
"purse": []
}
self.touch_template = {"head_touch": [], "body_touch": []}
self.emotion_template = {
"happiness": [],
"sadness": [],
"blink": [],
"surprise": [],
"fear": [],
"anger": [],
"disgust": [],
"contempt": []
}
self.sound_template = {"beep": [], "beep_beep": [], "clap": []}
self.pose_template = {
"hands_up": [],
"high_knee_left": [],
"high_knee_right": []
}
self.extra_template = {"face_on_floor": [], "light": []}
# The event template consists of the 5+1 categories (deepcopy, to create new lists)
self.event_template = {
"object/toys": copy.deepcopy(self.toy_template),
"touch": copy.deepcopy(self.touch_template),
"face/emotions": copy.deepcopy(self.emotion_template),
"sounds": copy.deepcopy(self.sound_template),
"poses": copy.deepcopy(self.pose_template),
"xtra": self.extra_template
}
# Prepare the other templates for hours and days
self.hour_template = {
"hour": [],
"predicted_pattern": [],
"observed_pattern": [],
"events": copy.deepcopy(self.event_template)
}
self.hours_template = [
copy.deepcopy(self.hour_template)
for _ in range(self.hours_per_day)
]
self.day_template = {
"day": [],
"day_group": [],
"hours": self.hours_template
}
# Start at day 0 and hour 0
self.current_day = 0
self.current_hour = 0
# The Program is not yet at the end
self.end = False
# The Program is not active yet
self.active = False
# Add a shutdown hook, to savely close the program (stop core behavior at shutdown)
rospy.on_shutdown(self.shutdow_hook)
# try to load the file
try:
with open(self.monitor_file, "r") as filehandle:
print("load the file")
self.monitor_list = json.load(filehandle)
# Increment the number of runs by one
self.col_id = self.monitor_list[0] + 1
self.monitor_list[0] = self.col_id
# if the file does not exist, create a new file
except IOError:
print("create a new file")
self.col_id = 0
self.monitor_list = [
self.col_id,
[copy.deepcopy(self.day_template) for _ in range(self.days)]
]
# Set all counters to 0, all 6 categories are in one dict to make it easier
self.counter_dict = {}
for topic_name in self.event_template:
for content_name in self.event_template[topic_name]:
self.counter_dict[content_name] = 0
# Prepare miro's topic
topic_root = "/miro/" + self.robot_name
# Set up all core publishers
self.pub_core_control = rospy.Publisher(topic_root + "/core/control",
core_control,
queue_size=0)
self.pub_core_config = rospy.Publisher(topic_root + "/core/config",
core_config,
queue_size=0)
# Wait the time specified in launch file. This is necessary, because the publishers may not be ready immediately
rospy.sleep(self.start_wait)
# After the waiting time, set the starting time
self.start_time = rospy.Time.now().to_sec()
# Set the dict to map from patterns to states
self.pattern_to_state_dict = {
"nothing": "Sleep",
"face_on_floor": "Angry_disturbed",
"happy_face": "Happy_active",
"body_touch": "Happy_active",
"head_touch": "Happy_calm",
"sad_face": "Angry_calm",
"liked": "Happy_active",
"disliked": "Angry_disturbed",
"strange1": "Neutral",
"strange2": "Angry_calm",
"sound": "Angry_disturbed",
"hands_up": "Happy_active",
"neutral": "Neutral"
}
# Set the dict to map from states to /core/control messages
self.state_dict = {
"Neutral": [0.5, 0.5, 0.5, 0.5, 1.0, 0.0],
"Angry_disturbed": [0.0, 1.0, 0.0, 1.0, 1.0, 0.0],
"Angry_calm": [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
"Happy_active": [1.0, 1.0, 1.0, 1.0, 1.0, 0.0],
"Happy_calm": [1.0, 0.0, 1.0, 0.0, 1.0, 0.0],
"Sleep": [0.5, 0.0, 0.5, 0.0, 0.0, 1.0]
}
# If the flag to use miro's demo behavior was set, activate the demo behavior
# This is almost the same as the normal demo mode in the gui application
# The expression through ears is deactivated, because otherwise the ears move alot
# And you have the option to activate or deactivate the move behavior
if self.demo_mode:
c = core_config()
c.P2B_W_signals = c.P2B_W_signals | miro.MIRO_P2B_W_BRANCH_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ADJUST_RTC ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_VALENCE_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_AROUSAL_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE_SLEEP ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_CLOCK ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_WAKEFULNESS ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_TOUCH
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_LIGHT
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SOUND
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_ACCEL
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SLEEP_BLOCKED
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_RANDOMIZE_VALENCE
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FAST_SLEEP_DYNAMICS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_LIGHT
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_TAIL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EYELIDS
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EARS ## ?
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_VOCAL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_BODY
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_DEBUG_SONAR
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_NO_PIRATE_NOISES
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_DO_PIRATE_NOISES
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_ENABLE
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DEBUG
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_FORCE_MULL
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_RANDOMIZE_ORIENT
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DISABLE_HALT
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_MODULATE_BY_SONAR
# Only activate demo movement if configured in the launch file
if self.demo_move:
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_ENABLE
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_RESET_KC_INTEGRATORS
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH ## no movement
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_MOTION
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_TRANSLATION
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_INTO_SONAR
# c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_POS_CONTROL
c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_CLIFF_REFLEX
c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_ENABLE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_AUDIO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_VIDEO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_PRIORITY
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_OTHER
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_REAFF_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_SUPPRESS
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_TEST_PATTERN
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_TEST_ALARM
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_NO_I2C_BUSY_ALARM
c.msg_flags = c.FLAG_UPDATE_SIGNALS
# Publish the core/control message to activate miro's core behavior
self.pub_core_config.publish(c)
# Add all other publishers
# Platform control (play a sound as a reaction)
self.joint_state = None
self.eyelid_closure = 0.0
self.platform_control_pub = rospy.Publisher(topic_root +
"/platform/control",
platform_control,
queue_size=0)
self.stop_sound_pub = rospy.Publisher("/stop/sound",
Int8,
queue_size=1)
self.stop_sound_sub = rospy.Subscriber("/stop/sound",
Int8,
self.callback_stop_sound,
queue_size=1)
# Cmd_vel (move as a reaction)
self.move_pub = rospy.Publisher(topic_root + "/control/cmd_vel",
Twist,
queue_size=0)
# Cosmetic joints (close eyes, etc. as reaction)
self.cosmetic_pub = rospy.Publisher(topic_root +
"/control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
# Lights (display lights as reaction)
self.lights_pub = rospy.Publisher(topic_root + "/control/lights",
UInt16MultiArray,
queue_size=0)
# Add all subscribers
# Core state, main logic + sleep state
self.awake = True
self.core_state = rospy.Subscriber(topic_root + "/core/state",
core_state,
self.callback_state,
queue_size=1)
# Face on the floor
self.face_sub = rospy.Subscriber("/faceCoord", Int32MultiArray,
self.callback_faces)
# Face emotions
if self.monitor_emotions:
self.face_sub = rospy.Subscriber("/faces", Faces,
self.callback_emotions)
# Platform sensors (used for Touch)
self.sub_platform_sensors = rospy.Subscriber(
topic_root + "/platform/sensors", platform_sensors,
self.callback_sensors)
# Poses (PoseNet Human Pose Analysis) -> HandsUp and HighKnees
self.keypoint_sub = rospy.Subscriber("/poses", Poses,
self.callback_poses)
# Toy recognition
self.toys_sub = rospy.Subscriber("/toys", ObjectSide,
self.callback_toys)
# Audio recogniton (Beep, BeepBeep and Clap) be aware of false positives
self.i = 0
self.max_count = 10
self.mic_array_1 = np.zeros(2000 * self.max_count)
self.mic_array_2 = np.zeros(2000 * self.max_count)
self.mics_sub = rospy.Subscriber(topic_root + "/platform/mics",
platform_mics, self.callback_mics)
# Predict the pattern for the first time interval
self.predict_next_pattern()
# stop all sounds (else it could happen that some old sounds from last time, start playing again)
rospy.sleep(1)
self.stop_sound_pub.publish(1)
def __init__(self):
rospy.init_node("miro_ros_client_py", anonymous=True)
self.active = False
self.robot_name = rospy.get_param("~robot_name")
self.demo_mode = rospy.get_param("~demo_mode")
self.id = 0
self.start_wait = rospy.get_param("~start_wait")
self.interval_time = rospy.get_param("~interval_time")
self.monitor_file = rospy.get_param("~monitor_file") + str(
self.interval_time) + ".log"
topic_root = "/miro/" + self.robot_name
self.dict_template = {
"col_id": [],
"timeStamp": [],
"happiness": [],
"sadness": [],
"blink": [],
"surprise": [],
"fear": [],
"anger": [],
"disgust": [],
"contempt": [],
"head_touch": [],
"body_touch": [],
"face_on_floor": []
}
self.start_time = rospy.Time.now().to_sec()
rospy.on_shutdown(self.shutdow_hook)
# try to load the file
try:
with open(self.monitor_file, "r") as filehandle:
print("load the file")
print(self.monitor_file)
# print(filehandle)
self.monitor_list = json.load(filehandle)
# print(self.monitor_list)
self.col_id = self.monitor_list[0] + 1
self.monitor_list[0] = self.col_id
except IOError:
print("create a new file")
self.col_id = 0
self.monitor_list = [
self.col_id, copy.deepcopy(self.dict_template)
]
self.happiness = 0
self.sadness = 0
self.blink = 0
self.surprise = 0
self.fear = 0
self.anger = 0
self.disgust = 0
self.contempt = 0
self.face_on_floor = 0
self.head_touch = 0
self.body_touch = 0
self.pub_core_control = rospy.Publisher(topic_root + "/core/control",
core_control,
queue_size=0)
self.pub_core_config = rospy.Publisher(topic_root + "/core/config",
core_config,
queue_size=0)
# wait a specific amount of time
rospy.sleep(self.start_wait)
# send specific core_control message to start MiRo in a neutral state
q = core_control()
q.mood_drive_target.valence = 0.5
q.mood_drive_target.arousal = 0.5
q.mood_drive_gamma = 1
q.emotion_drive_target.valence = 0.5
q.emotion_drive_target.arousal = 0.5
q.emotion_drive_gamma = 1
self.pub_core_control.publish(q)
# prepare general core_control message
self.core_message = core_control()
# self.core_message.msg_flags = core_control.FLAG_SYNC_PLATFORM | core_control.FLAG_SYNC_CORE
self.core_message.emotion_drive_target.valence = 0.5
self.core_message.emotion_drive_target.arousal = 0.5
self.core_message.emotion_drive_gamma = 0.5
# if flag was set activate demo mode
if self.demo_mode == True:
c = core_config()
c.P2B_W_signals = c.P2B_W_signals | miro.MIRO_P2B_W_BRANCH_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ADJUST_RTC ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_VALENCE_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_AROUSAL_DYNAMICS
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_ENABLE_SLEEP ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_CLOCK ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_WAKEFULNESS ##
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_TOUCH
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_LIGHT
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SOUND
c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_ACCEL
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FROM_SLEEP_BLOCKED
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_RANDOMIZE_VALENCE
# c.P2U_W_affect_signals = c.P2U_W_affect_signals | miro.MIRO_P2U_W_AFFECT_FAST_SLEEP_DYNAMICS
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_ENABLE
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_LIGHT
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_TAIL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EYELIDS
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_EARS ## ?
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_VOCAL
c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_THROUGH_BODY
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_DEBUG_SONAR
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_NO_PIRATE_NOISES
# c.P2U_W_express_signals = c.P2U_W_express_signals | miro.MIRO_P2U_W_EXPRESS_DO_PIRATE_NOISES
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_ENABLE
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DEBUG
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_FORCE_MULL
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_RANDOMIZE_ORIENT
# c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_DISABLE_HALT
c.P2U_W_action_signals = c.P2U_W_action_signals | miro.MIRO_P2U_W_ACTION_MODULATE_BY_SONAR
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_ENABLE
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_RESET_KC_INTEGRATORS
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH ## no movement
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_MOTION
# c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_TRANSLATION
c.P2U_W_body_signals = c.P2U_W_body_signals | miro.MIRO_P2U_W_BODY_NO_PUSH_INTO_SONAR
# c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_POS_CONTROL
c.P2L_W_signals = c.P2L_W_signals | miro.MIRO_P2L_W_ENABLE_CLIFF_REFLEX
c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_ENABLE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_AUDIO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_IGNORE_VIDEO
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_PRIORITY
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SEND_OTHER
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_REAFF_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_NO_SUPPRESS
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_COMPROMISE
# c.P2S_W_signals = c.P2S_W_signals | miro.MIRO_P2S_W_SPATIAL_SHOW_TEST_PATTERN
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_TEST_ALARM
# c.P1_W_signals = c.P1_W_signals | miro.MIRO_P1_W_NO_I2C_BUSY_ALARM
c.msg_flags = c.FLAG_UPDATE_SIGNALS
self.pub_core_config.publish(c)
# listen to the core to adjust the state
self.sub_core_state = rospy.Subscriber(topic_root + "/core/state",
core_state,
self.callback_core_state)
# subscribe to the platform state topic to get the time
self.platform_state = rospy.Subscriber(topic_root + "/platform/state",
platform_state,
self.callback_state,
queue_size=1)
# other subscribers
# face lying down
self.face_sub = rospy.Subscriber("/faceCoord", Int32MultiArray,
self.callback_faces)
# face emotions
self.face_sub = rospy.Subscriber("/faces", Faces,
self.callback_emotions)
# touch
self.sub_platform_sensors = rospy.Subscriber(
topic_root + "/platform/sensors", platform_sensors,
self.callback_sensors)
def shutdow_hook(self):
# stop core behavior
c = core_config()
c.msg_flags = c.FLAG_UPDATE_SIGNALS
self.pub_core_config.publish(c)
