class PrimaryStatesServer():
"""Primary States Server"""
def __init__(self, database_file=""):
self.action_name = rospy.get_name()
self.MAP_NAME = "glassgarden.map"
self.PLANNING_TIMEOUT = 10
self.RATE_ACTIONLOOP = rospy.Rate(4) # Hz
self.next_location = ""
self.current_emotional_state = "neutral"
self.current_state = ""
self.database_handler = DatabaseHandler(filename=database_file)
self.publisher_emotional_feedback = rospy.Publisher(
"/cyborg_controller/emotional_feedback",
EmotionalFeedback,
queue_size=100)
self.subscriber_emotional_state = rospy.Subscriber(
"/cyborg_controller/emotional_state",
EmotionalState,
self.emotional_callback,
queue_size=10)
self.publisher_event = rospy.Publisher(
"/cyborg_controller/register_event", String, queue_size=100)
self.publisher_location_command = rospy.Publisher(
"/cyborg_behavior/command_location", String, queue_size=10)
self.client_behavior = actionlib.SimpleActionClient(
"/cyborg_behavior", StateMachineAction)
self.server_primary_states = actionlib.SimpleActionServer(
self.action_name,
StateMachineAction,
execute_cb=self.callback_server_primary_states,
auto_start=False)
self.server_primary_states.start()
rospy.loginfo("PrimaryStatesServer: Initiated")
def callback_server_primary_states(self, goal):
self.state_goal = goal
if self.state_goal.current_state == "wandering_emotional":
self.wandering_emotional()
elif self.state_goal.current_state == "navigation_planning":
self.navigation_planning_state(self.state_goal)
def emotional_callback(self, message):
self.current_emotional_state = message.to_emotional_state
def create_and_send_behavior_goal(self, behavior, location=None):
# Create and send goal to behaviorserver
goal = StateMachineGoal()
goal.current_state = behavior
if location != None:
goal.order = location
self.client_behavior.send_goal(goal)
# Check execution of action
def actionloop_check(self):
# Check if preempt is requested
if self.server_primary_states.is_preempt_requested():
self.client_behavior.cancel_all_goals()
self.server_primary_states.set_preempted()
rospy.loginfo("PrimaryStatesServer: %s state preempted.",
self.current_state)
return True
else:
# Check behaviorserver state
behaviorserver_state = self.client_behavior.get_state()
# not active
if behaviorserver_state == 3:
# succeeded
rospy.loginfo(
"PrimaryStatesServer: %s state BehaviorServer goal succeeded. ",
self.current_state)
self.server_primary_states.set_succeeded()
return True
elif behaviorserver_state == 2:
# Preempted
rospy.loginfo(
"PrimaryStatesServer: %s state BehaviorServer goal was preempted. ",
self.current_state)
self.server_primary_states.set_preempted()
return True
elif behaviorserver_state == 4:
# aborted
rospy.loginfo(
"PrimaryStatesServer: %s state BehaviorServer goal aborted. ",
self.current_state)
self.server_primary_states.set_aborted()
return True
def wandering_emotional(self):
rospy.loginfo("PrimaryStatesServer: Executing wander emotional state.")
#connect to server and send goal
goal = "wandering_emotional"
self.create_and_send_behavior_goal(goal)
while not rospy.is_shutdown():
if self.actionloop_check() == True:
return
elif self.current_emotional_state not in [
"bored", "curious", "unconcerned"
]: # emotions currently not integrated in server
self.client_behavior.cancel_all_goals()
rospy.sleep(2)
self.server_primary_states.set_succeeded()
rospy.loginfo(
"PrimaryStatesServer: wandering complete, preempted.")
return
self.RATE_ACTIONLOOP.sleep()
# set terminal goal status in case of shutdown
self.server_primary_states.set_aborted()
# Called when the controller (state machine) sets the navigation_planning state as active
def navigation_planning_state(self, goal):
rospy.loginfo(
"PrimaryStatesServer: Executing navigation_planning state.")
time.sleep(1) # let roscore update connections
self.next_location = None
if goal.event == "navigation_schedular":
if self.current_emotional_state == "angry":
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.MAP_NAME, crowded=False)
message = String(data=self.next_location.location_name)
self.publisher_location_command.publish(message)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving_schedular")
else:
self.next_location = self.database_handler.search_ongoing_events(
robot_map_name=self.MAP_NAME,
current_date=datetime.datetime.now())
message = String(data=self.next_location.location_name)
self.publisher_location_command.publish(message)
self.send_emotion(pleasure=0, arousal=0, dominance=-0.1)
self.change_state(event="navigation_start_moving_schedular")
elif goal.event == "navigation_emotional":
if self.current_emotional_state in [
"angry", "sad", "fear", "inhibited"
]:
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.MAP_NAME, crowded=False)
message = String(data=self.next_location.location_name)
self.publisher_location_command.publish(message)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving_emotional")
elif self.current_emotional_state in [
"happy", "loved", "dignified", "neutral", "elated"
]:
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.MAP_NAME, crowded=True)
message = String(data=self.next_location.location_name)
self.publisher_location_command.publish(message)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving_emotional")
elif self.current_emotional_state in [
"bored", "curious", "unconcerned"
]:
self.next_location = "wandering"
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_wandering")
def change_state(self, event=None):
if event == None:
self.server_primary_states.set_aborted()
return
else:
if event == "navigation_start_moving":
if self.next_location != None:
self.publisher_event.publish(event)
else:
self.publisher_event.publish(event)
# wait until state is preempted, or abort if it takes too long
started_waiting = time.time()
while not rospy.is_shutdown():
if self.server_primary_states.is_preempt_requested():
self.server_primary_states.set_preempted()
return
elif (time.time() - started_waiting > self.PLANNING_TIMEOUT):
self.server_primary_states.set_aborted()
return
self.RATE_ACTIONLOOP.sleep()
# set terminal goal status in case of shutdown
self.server_primary_states.set_aborted()
# Sends the emotional change to the controller
def send_emotion(self, pleasure, arousal, dominance):
msg = EmotionalFeedback()
msg.delta_pleasure = pleasure
msg.delta_arousal = arousal
msg.delta_dominance = dominance
self.publisher_emotional_feedback.publish(msg)
class NavigationServer():
"""NavigationServer"""
client_base_feedback = StateMachineFeedback()
client_base_result = StateMachineResult()
next_location = None
command_location = None
current_location = None
reason = ""
map_name = "ntnu2.map"
base_canceled = False
base_succeded = False
current_x = 0.0
current_y = 0.0
current_emotion = "neutral"
planing_timeout = 60 # (s)
moving_timeout = 1000 # (s)
taking_timeout = 60 # (s)
def __init__(self, database_file=""):
self.scheduler_rate = rospy.Rate(1) # (hz)
self.server_rate = rospy.Rate(0.5) # (hz)
self.server_planing = actionlib.SimpleActionServer(
rospy.get_name() + "/planing",
StateMachineAction,
execute_cb=self.server_planing_callback,
auto_start=False)
self.server_moving = actionlib.SimpleActionServer(
rospy.get_name() + "/moving",
StateMachineAction,
execute_cb=self.server_moving_callback,
auto_start=False)
self.server_talking = actionlib.SimpleActionServer(
rospy.get_name() + "/talking",
StateMachineAction,
execute_cb=self.server_talking_callback,
auto_start=False)
self.server_go_to = actionlib.SimpleActionServer(
rospy.get_name() + "/go_to",
NavigationGoToAction,
execute_cb=self.server_go_to_callback,
auto_start=False)
self.server_planing.start()
self.server_moving.start()
self.server_talking.start()
self.server_go_to.start()
self.client_base = actionlib.SimpleActionClient(
"/rosarnl_node/move_base", MoveBaseAction)
self.emotion_publisher = rospy.Publisher(
"/cyborg_controller/emotional_feedback",
EmotionalFeedback,
queue_size=100)
self.event_publisher = rospy.Publisher(
"/cyborg_controller/register_event", String, queue_size=100)
self.speech_publisher = rospy.Publisher(
"/cyborg_text_to_speech/text_to_speech", String, queue_size=100)
self.database_handler = DatabaseHandler(filename=database_file)
self.location_subscriber = rospy.Subscriber("/rosarnl_node/amcl_pose",
PoseWithCovarianceStamped,
self.location_callback)
self.emotion_subscriber = rospy.Subscriber(
"/cyborg_controller/emotional_state",
EmotionalState,
self.emotion_callback,
queue_size=100)
self.text_subscriber = rospy.Subscriber("/text_from_speech",
String,
self.text_callback,
queue_size=100)
self.scheduler_thread = threading.Thread(target=self.scheduler)
self.scheduler_thread.daemon = True # Thread terminates when main thread terminates
self.scheduler_thread.start()
rospy.loginfo("NavigationServer: Activated.")
# Updates the current position when the position subscriber receives data
def location_callback(self, data):
self.current_x = data.pose.pose.position.x
self.current_y = data.pose.pose.position.y
# Updates the current emotion when the emotion subscriber recives data from the controller (emotion system)
def emotion_callback(self, data):
self.current_emotion = data.to_emotional_state
# Thread, updating current location name based on current possition, checks for ongoing events and current position compared to the event, if ongoing event is an other location it publish a navigation_schedulaer event for the state machine
def scheduler(self): # Threaded
while (not rospy.is_shutdown()):
self.current_location = self.database_handler.find_location(
robot_map_name=self.map_name,
location_x=self.current_x,
location_y=self.current_y)
current_location_name = self.current_location.location_name if self.current_location != None else ""
event = self.database_handler.search_ongoing_events(
robot_map_name=self.map_name,
current_date=datetime.datetime.now())
if event != None:
if event.location_name != current_location_name:
self.event_publisher.publish("navigation_schedualer")
self.scheduler_rate.sleep()
# Called once when the robot base (ROSARNL) goal completes
def client_base_done_callback(self, state, result):
if self.is_controlling_base:
if (state == 3): # Succeded aka arived at location
self.base_succeded = True
elif (state == 2): # Happens when we cancel goals aka preemted
self.base_canceled = True
elif (state == 1): # Canceled?
self.base_canceled = True
self.client_base_state = state
self.client_base_result = result
rospy.logdebug(
"NavigationServer: Base has completed its execution with " +
str(state) + " and result " + str(result) + ".")
# Called once when the robot base (ROSARNL) goal becomes active
def client_base_active_callback(self):
rospy.logdebug("NavigationServer: Base goal has gone active.")
self.is_controlling_base = True
# Called every time feedback is received for the goal for the robot base (ROSARNL)
def client_base_feedback_callback(self, feedback):
rospy.logdebug("NavigationServer: Received feedback from base - " +
str(feedback) + ".")
self.client_base_feedback = feedback
# Called when the controller (state machine) sets the navigation_planing state as active
def server_planing_callback(self, goal):
rospy.logdebug("NavigationServer: Executing planing state.")
time.sleep(2) # Let roscore update connections
# Select what to do based on event
self.next_location = None
if goal.event == "navigation_schedualer":
if self.current_emotion == "angry":
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.map_name, crowded=False)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving")
else:
self.next_location = self.database_handler.search_ongoing_events(
robot_map_name=self.map_name,
current_date=datetime.datetime.now())
self.send_emotion(pleasure=0, arousal=0, dominance=-0.1)
self.change_state(event="navigation_start_moving")
elif goal.event == "navigation_emotional":
if self.current_emotion in ["angry", "sad", "fear", "inhibited"]:
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.map_name, crowded=False)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving")
elif self.current_emotion in [
"happy", "loved", "dignified", "neutral", "elated"
]:
self.next_location = self.database_handler.search_for_crowded_locations(
robot_map_name=self.map_name, crowded=True)
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_moving")
elif self.current_emotion in ["bored", "curious", "unconcerned"]:
self.next_location = "wandering"
self.send_emotion(pleasure=0, arousal=0, dominance=0.1)
self.change_state(event="navigation_start_wandering")
elif goal.event == "navigation_command":
self.next_location = self.command_location
self.send_emotion(pleasure=0, arousal=0, dominance=-0.2)
self.change_state(event="navigation_start_moving")
# Publishes an event and waits for a change of state.
def change_state(self, event=None):
if event != None and self.next_location != None:
self.event_publisher.publish(event)
else:
self.server_planing.set_aborted()
return
# Wait until state is preemted, or abort if it takes to long time
started_waiting = time.time() # Prevent eternal looping
while not rospy.is_shutdown():
if self.server_planing.is_preempt_requested():
self.server_planing.set_preempted()
return
elif (time.time() - started_waiting >
self.planing_timeout): # Prevent eternal looping
self.server_planing.set_aborted()
return
self.server_rate.sleep()
# Called when the controller (state machine) sets the navigation_moving state as active
def server_moving_callback(self, goal):
rospy.logdebug("NavigationServer: Executing moving state." +
" Navigation movinging cmd " + str(self.next_location))
self.goal = goal
self.base_canceled = False
self.base_succeded = False
self.is_controlling_base = False
# Select how to move based on event
if goal.event == "navigation_start_wandering":
self.start_wandering()
elif goal.event == "navigation_start_moving" and self.next_location != None:
self.start_moving()
else:
self.server_moving.set_aborted()
rospy.logdebug(
"NavigationServer: Received event that cant be handled - " +
str(goal.event) + ".")
# The robot base starts to wander and keeps wandering until it is preemted or no longer in the right mood
def start_wandering(self):
# Tell base to start wandering
rospy.logdebug("NavigationServer: Waiting for base wandering service.")
rospy.wait_for_service("/rosarnl_node/wander")
rospy.logdebug("NavigationServer: wandering service available.")
try:
baseStartWandering = rospy.ServiceProxy("/rosarnl_node/wander",
Empty)
baseStartWandering()
except rospy.ServiceException, e:
rospy.logdebug("NavigationServer: wandering service error - " +
str(e))
started_waiting = time.time() # Prevent eternal looping
feedback_cycle = time.time()
while not rospy.is_shutdown():
if self.server_moving.is_preempt_requested():
self.client_base.cancel_all_goals(
) # HERE - goal on wandering?
try:
baseStop = rospy.ServiceProxy("/rosarnl_node/stop", Empty)
baseStop()
except rospy.ServiceException, e:
rospy.logdebug("NavigationServer: stop service error - " +
str(e))
self.server_moving.set_preempted()
return
if self.current_emotion not in ["bored", "curious", "unconcerned"]:
self.client_base.cancel_all_goals(
) # HERE - goal on wandering?
try:
baseStop = rospy.ServiceProxy("/rosarnl_node/stop", Empty)
baseStop()
except rospy.ServiceException, e:
rospy.logdebug("NavigationServer: stop service error - " +
str(e))
self.event_publisher.publish("navigation_wandering_completed")
# Wait until state is preemted
started_waiting = time.time() # Prevent eternal looping
while not rospy.is_shutdown():
if self.server_moving.is_preempt_requested():
self.server_moving.set_preempted()
return
self.server_rate.sleep()
return