#!/usr/bin/env python import roslib; roslib.load_manifest('maiss_decision_making') import rospy import smach import smach_ros import std_srvs.srv as std_srvs import std_msgs from geometry_msgs.msg import * from move_base_msgs.msg import * from markov_decision_making.msg import ActionInfo from maiss_msg.msg import * from maiss_nav.msg import * from maiss_hri.msg import * from smach_ros import SimpleActionState import math import threading class MonitorActiveAssistanceRequests(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['cleared_to_act','cleared_but_no_event','preempted'], output_keys=['assistance_goal_x', 'assistance_goal_y']) s = rospy.Service('act', std_srvs.Empty, self.server_callback) self.wakeup = threading.Event() self.ongoing_emergency = False self.cleared_to_act = False def assistance_callback(self, msg, userdata): if msg.event.type == MaissControlEvent.WAVING: goal_position = MoveToEventPositionActionGoal() userdata.assistance_goal_x = msg.event_position.x; userdata.assistance_goal_y = msg.event_position.y; self.ongoing_assistance_request = True; elif msg.event.type == MaissControlEvent.PERSON_ASSISTANCE_RESOLVED: self.ongoing_assistance_request = False; def server_callback(self, req): self.cleared_to_act = True self.wakeup.set() return std_srvs.EmptyResponse() def execute(self, userdata): self.cleared_to_act = False self.ongoing_assistance_request = False rospy.Subscriber("/maiss_control_events", MaissControlEvent, self.assistance_callback, userdata) while not (self.cleared_to_act): self.wakeup.wait() self.wakeup.clear() #Blocking until callback triggers if self._preempt_requested: userdata.assistance_goal_x = 0 userdata.assistance_goal_y = 0 return 'preempted' if(self.ongoing_assistance_request): return 'cleared_to_act' else: return 'cleared_but_no_event' def request_preempt(self): self._preempt_requested = True self.wakeup.set() class ConfirmClosest(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['closest','not_closest'], input_keys=['assistance_goal_x', 'assistance_goal_y'], output_keys=['assistance_goal_x', 'assistance_goal_y']) self.poses_sub = rospy.Subscriber("team/poses", TeamPoses, self.poses_callback) self.action_sub = rospy.Subscriber("decision_making/top_level_POMDP/action", ActionInfo, self.action_callback) self.agent_id = rospy.get_param('agent_id',0) self.current_action = 0 self.latest_poses = TeamPoses() self.poses_known = False def action_callback(self, msg): self.current_action = msg.action def poses_callback(self, msg): self.latest_poses = msg self.poses_known = True def execute(self, userdata): if self.current_action != 5: #5 = assistance_response-assistance_response rospy.logwarn("I am closest because the other agent did not select the assistance response task.") return 'closest' if self.poses_known: d_min = 1000.0 closest_agent = 0 agent_idx = 0 for p in self.latest_poses.data: d = math.hypot(p.pose.pose.position.x-userdata.assistance_goal_x, p.pose.pose.position.y-userdata.assistance_goal_y) if d < d_min: d_min = d closest_agent = agent_idx agent_idx += 1 if self.latest_poses.id[closest_agent] == self.agent_id: return 'closest' else: rospy.logwarn("Tried to determine closest agent, but I had no team poses.") rospy.loginfo("This agent is not the closest. The closest is agent %s",self.latest_poses.id[closest_agent]) return 'not_closest' class SignalCompletion(smach.State): def __init__(self): agent_id = rospy.get_param('agent_id',0) smach.State.__init__(self, outcomes=['task_completed']) self.det_pub = rospy.Publisher('/maiss_control_events', MaissControlEvent) self.hri_pub = rospy.Publisher('/agent/'+str(agent_id)+'/HRI_request', HRITaskRequest) def execute(self, userdata): agent_id = rospy.get_param('agent_id',0) d = MaissControlEvent() d.event.type = MaissControlEvent.PERSON_ASSISTANCE_RESOLVED d.event.id = agent_id d.event.timestamp = rospy.Time.now() self.det_pub.publish(d) req = HRITaskRequest() req.request = HRITaskRequest.GUIDANCE_FINISHED self.hri_pub.publish(req) rospy.sleep(0.1) return 'task_completed' class SignalCancel(smach.State): def __init__(self): agent_id = rospy.get_param('agent_id',0) smach.State.__init__(self, outcomes=['task_cancelled']) self.control_pub = rospy.Publisher('/maiss_control_events', MaissControlEvent) self.hri_pub = rospy.Publisher('/agent/'+str(agent_id)+'/HRI_request', HRITaskRequest) def execute(self, userdata): agent_id = rospy.get_param('agent_id',0) d = MaissControlEvent() d.event.type = MaissControlEvent.ROBOT_TASK_CANCELLED d.event.id = agent_id d.event.timestamp = rospy.Time.now() self.control_pub.publish(d) req = HRITaskRequest() req.request = HRITaskRequest.GUIDANCE_CANCELLED self.hri_pub.publish(req) rospy.sleep(0.1) return 'task_cancelled' class AssistanceInteraction(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['guidance_requested','assistance_complete','preempted','aborted'], output_keys=['destination']) agent_id = rospy.get_param('agent_id',0) self.destination = PoseStamped() self.sub = rospy.Subscriber('/agent/'+str(agent_id)+'/HRI_reply', HRITaskReply, self.HRI_reply_callback) self.pub = rospy.Publisher('/agent/'+str(agent_id)+'/HRI_request', HRITaskRequest) self.wakeup = threading.Event() self.cancel = False self.HRI_resolved = False def HRI_reply_callback(self, msg): self.destination = PoseStamped() if(msg.reply == HRITaskReply.GUIDE_TO_LOCATION): self.destination.pose.position.x = msg.data_vec[0] self.destination.pose.position.y = msg.data_vec[1] self.destination.pose.orientation.w = 1.0 self.destination.header.stamp = rospy.get_rostime() self.destination.header.frame_id = "/map" self.HRI_resolved = True self.wakeup.set() if(msg.reply == HRITaskReply.CANCEL_HRI): self.cancel = True self.wakeup.set() def execute(self, userdata): self.cancel = False self.HRI_resolved = False userdata.destination = PoseStamped() if(self.pub.get_num_connections() == 0): #noone to listen to our HRI request. Drop task. rospy.logwarn("No HRI client. Aborting behavior") return 'aborted' msg = HRITaskRequest() msg.request = HRITaskRequest.ASSISTANCE_REQUEST self.pub.publish(msg) while not (self.HRI_resolved or self.cancel): self.wakeup.wait() self.wakeup.clear() if self.cancel == True: return 'preempted' else: userdata.destination = self.destination return 'guidance_requested' class AnnounceVisitorAssitance(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['person_found','cancelled','preempted']) agent_id = rospy.get_param('agent_id',0) self.sub = rospy.Subscriber('/agent/'+str(agent_id)+'/HRI_reply', HRITaskReply, self.HRI_reply_callback) self.pub = rospy.Publisher('/agent/'+str(agent_id)+'/HRI_request', HRITaskRequest) self.wakeup = threading.Event() self.person_found = False self.cancel = False def HRI_reply_callback(self, msg): if(msg.reply == HRITaskReply.PERSON_PRESENT): self.person_found = True self.wakeup.set() if(msg.reply == HRITaskReply.CANCEL_HRI): self.cancel = True self.wakeup.set() def execute(self, userdata): if(self.pub.get_num_connections() == 0): #noone to listen to our HRI request. Drop task. rospy.logwarn("No HRI client. Aborting behavior") return 'cancelled' msg = HRITaskRequest() msg.request = HRITaskRequest.ANNOUNCE_ASSISTANCE self.pub.publish(msg) while not (self.person_found or self.cancel or self._preempt_requested): self.wakeup.wait() self.wakeup.clear() if self.cancel == True: return 'cancelled' elif self._preempt_requested == True: return 'preempted' else: return 'person_found' def request_preempt(self): self._preempt_requested = True self.wakeup.set() def main(): rospy.init_node('assitance_response_smach') #sm_main: Encapsulates the whole task and keeps looping (no I/O) sm_main = smach.StateMachine(outcomes=[]) with sm_main: smach.StateMachine.add('Monitor Active Assistance Requests', MonitorActiveAssistanceRequests(), transitions={'cleared_to_act':'Confirm Closest Agent', 'cleared_but_no_event':'Search for Visitors', 'preempted':'Monitor Active Assistance Requests'}) con_search_people = smach.Concurrence(outcomes=['person_found','no_person_found','cancelled'], default_outcome='cancelled', outcome_map={'person_found':{'Announce Visitor Assistance':'person_found'}, 'no_person_found':{'Move to Elevator Hallway':'succeeded'}, 'cancelled':{'Announce Visitor Assistance':'cancelled'}, 'cancelled':{'Move to Elevator Hallway':'preempted'}, 'cancelled':{'Move to Elevator Hallway':'aborted'}}, child_termination_cb=(lambda so: True)) with con_search_people: smach.Concurrence.add('Announce Visitor Assistance', AnnounceVisitorAssitance()) elevator_hallway_target = MoveBaseGoal() elevator_hallway_target.target_pose.pose.position.x = 3.0 elevator_hallway_target.target_pose.pose.position.y = -2.0 elevator_hallway_target.target_pose.pose.orientation.w = 1.0 elevator_hallway_target.target_pose.header.stamp = rospy.get_rostime() elevator_hallway_target.target_pose.header.frame_id = "/map" smach.Concurrence.add('Move to Elevator Hallway', SimpleActionState('move_base', MoveBaseAction, goal=elevator_hallway_target)) smach.StateMachine.add('Search for Visitors', con_search_people, transitions={'person_found':'Interact', 'no_person_found':'Signal Cancel', 'cancelled':'Monitor Active Assistance Requests'}) smach.StateMachine.add('Confirm Closest Agent', ConfirmClosest(), transitions={'closest':'Move to Assistance Request', 'not_closest':'Signal Cancel'}) smach.StateMachine.add('Move to Assistance Request', SimpleActionState('move_to_event_position', MoveToEventPositionAction, goal_slots=['x','y']), transitions={'succeeded':'Interact', 'preempted':'Monitor Active Assistance Requests', 'aborted':'Monitor Active Assistance Requests'}, remapping={'x':'assistance_goal_x', 'y':'assistance_goal_y'}) smach.StateMachine.add('Interact', AssistanceInteraction(), transitions={'guidance_requested':'Guide to Destination', 'assistance_complete':'Signal Completion', #'deliver_message':'Deliver Message', 'preempted':'Signal Completion', 'aborted':'Signal Cancel'}) smach.StateMachine.add('Guide to Destination', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']), transitions={'succeeded':'Signal Completion', 'preempted':'Signal Cancel', 'aborted':'Signal Cancel'}, remapping={'target_pose':'destination'}) smach.StateMachine.add('Signal Cancel', SignalCancel(), transitions={'task_cancelled':'Monitor Active Assistance Requests'}) smach.StateMachine.add('Signal Completion', SignalCompletion(), transitions={'task_completed':'Monitor Active Assistance Requests'}) # Create and start the introspection server agent_id = rospy.get_param('agent_id',0) sis = smach_ros.IntrospectionServer('assistance_response_smach_viewer_'+`agent_id`, sm_main, '/SM_ASSISTANCE_'+`agent_id`) sis.start() outcome = sm_main.execute() if __name__ == '__main__': main()