#!/usr/bin/env python import roslib; roslib.load_manifest('maiss_decision_making') import rospy import math import smach import smach_ros import std_srvs.srv as std_srvs import std_msgs import signal import sys from geometry_msgs.msg import * from move_base_msgs.msg import * from maiss_msg.msg import * from maiss_nav.msg import * from maiss_hri.msg import * from smach_ros import SimpleActionState import threading # define state Monitor Incidents class MonitorActiveIncidents(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['person_trespassing','camera_failure','preempted','terminated'], output_keys=['person_trespassing_x', 'person_trespassing_y', 'camera_replacement_pose']) s = rospy.Service('act', std_srvs.Empty, self.server_callback) self.wakeup = threading.Event() self.ongoing_uncommon_incident = False self.cleared_to_act = False self.camera_recovery_distances = rospy.get_param('~camera_recovery_distances') self.active_event = MaissHeader() def uncommon_incident_callback(self, msg, userdata): if msg.event.type == MaissControlEvent.PERSON_TRESPASSING or msg.event.type == MaissControlEvent.SENSOR_FAILURE: self.active_event = msg.event self.ongoing_uncommon_incident = True if(msg.event.type == MaissControlEvent.PERSON_TRESPASSING): userdata.person_trespassing_x = msg.event_position.x userdata.person_trespassing_y = msg.event_position.y else: replacement_pose = PoseStamped() camera = str(self.active_event.id) d = self.camera_recovery_distances[camera] #Magic transform comes from poorly handled camera-to-world frame coordinates x = - rospy.get_param('/camera_params/'+camera+'/x') - 6.722 y = - rospy.get_param('/camera_params/'+camera+'/y') + 7.204 th = rospy.get_param('/camera_params/'+camera+'/th') x = x + d*math.cos(th) y = y + d*math.sin(th) replacement_pose.pose.position.x = x replacement_pose.pose.position.y = y replacement_pose.pose.orientation.w = math.cos (th / 2.0) replacement_pose.pose.orientation.z = math.sin (th / 2.0) replacement_pose.header.stamp = rospy.get_rostime() replacement_pose.header.frame_id = "/map" userdata.camera_replacement_pose = replacement_pose elif ((msg.event.type == MaissControlEvent.PERSON_TRESPASSING_RESOLVED and self.active_event.type == MaissControlEvent.PERSON_TRESPASSING) or (msg.event.type == MaissControlEvent.SENSOR_CONNECTED and self.active_event.type == MaissControlEvent.SENSOR_FAILURE)): self.ongoing_uncommon_incident = 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_uncommon_incident = False rospy.Subscriber("/maiss_control_events", MaissControlEvent, self.uncommon_incident_callback, userdata) while not (self.ongoing_uncommon_incident and self.cleared_to_act): self.wakeup.wait() self.wakeup.clear() #Blocking until callback triggers if self._preempt_requested: userdata.person_trespassing_x = 0 userdata.person_trespassing_y = 0 return 'preempted' if self.active_event.type == MaissControlEvent.PERSON_TRESPASSING: return 'person_trespassing' else: return 'camera_failure' def request_preempt(self): self._preempt_requested = True self.wakeup.set() class TrespassingInteraction(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['completed','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.complete = False self.cancel = False def HRI_reply_callback(self, msg): if(msg.reply == HRITaskReply.TRESPASSING_HRI_COMPLETE): self.complete = 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.TRESPASSING_HRI self.pub.publish(msg) while not (self.cancel or self.complete 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 'completed' def request_preempt(self): self._preempt_requested = True self.wakeup.set() class SignalCompletion(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['task_completed']) self.pub = rospy.Publisher('/maiss_control_events', MaissControlEvent) def execute(self, userdata): agent_id = rospy.get_param('agent_id',0) d = MaissControlEvent() d.event.type = MaissControlEvent.PERSON_TRESPASSING_RESOLVED d.event.id = agent_id d.event.timestamp = rospy.Time.now() self.pub.publish(d) rospy.sleep(0.1) return 'task_completed' def signal_handler(signal, frame): sys.exit(0) def main(): rospy.init_node('uncommon_incident_response_smach') signal.signal(signal.SIGINT, signal_handler) ##sm_main: Encapsulates the whole task and keeps looping (no I/O except SIGINT) sm_main = smach.StateMachine(outcomes=['terminated']) with sm_main: smach.StateMachine.add('Monitor Active Incidents', MonitorActiveIncidents(), transitions={'person_trespassing':'Move to Incident', 'camera_failure':'Replace Camera', 'preempted':'Monitor Active Incidents', 'terminated':'terminated'}) smach.StateMachine.add('Replace Camera', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']), transitions={'succeeded':'Monitor Active Incidents', 'preempted':'Monitor Active Incidents', 'aborted':'Monitor Active Incidents'}, remapping={'target_pose':'camera_replacement_pose'}) smach.StateMachine.add('Move to Incident', SimpleActionState('move_to_event_position', MoveToEventPositionAction, goal_slots=['x','y']), transitions={'succeeded':'Trespassing HRI', 'preempted':'Monitor Active Incidents', 'aborted':'Monitor Active Incidents'}, remapping={'x':'person_trespassing_x', 'y':'person_trespassing_y'}) smach.StateMachine.add('Trespassing HRI', TrespassingInteraction(), transitions={'completed':'Signal Completion', 'preempted':'Monitor Active Incidents', 'cancelled':'Monitor Active Incidents'}) smach.StateMachine.add('Signal Completion', SignalCompletion(), transitions={'task_completed':'Monitor Active Incidents'}) # Create and start the introspection server agent_id = rospy.get_param('agent_id',0) sis = smach_ros.IntrospectionServer('uncommon_incident_smach_viewer_'+`agent_id`, sm_main, '/SM_UNCOMMON_'+`agent_id`) sis.start() outcome = sm_main.execute() rospy.spin() sis.stop() if __name__ == '__main__': main()