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)