def __init__(self,msg=None):
        State.__init__(self, outcomes=['succeeded',"succeeded",'preempted'])
#        self.pub=rospy.Publisher('serial_switch_model',xm_SerialSwitchMode,queue_size=5 )
#        self.msg=xm_SerialSwitchMode()
#        self.msg.data=msg
        self.ser=rospy.ServiceProxy("serialswitchmode",xm_SerialSwitchMode)
        self.msg = msg
Пример #2
0
    def __init__(self, speed, goal):
        State.__init__(self, outcomes=["full_rotate", "not_full_rotate"])

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = "/odom"

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, "/base_footprint", rospy.Time(), rospy.Duration(1.0))
            self.base_frame = "/base_footprint"
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, "/base_link", rospy.Time(), rospy.Duration(1.0))
                self.base_frame = "/base_link"
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")

        self.cmd_vel = rospy.Publisher("cmd_vel", Twist)

        self.r = rospy.Rate(20)
        self.angular_speed = speed
        self.angular_tolerance = radians(2.5)
        self.goal_angle = goal
    def __init__(self):
        State.__init__(self, outcomes=['succeeded','aborted','preempted'], input_keys=['speed','distance'])

        self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
        self.pause_pub = rospy.Publisher('/PETIT/pause_pathwrapper', Empty)
        self.resume_pub = rospy.Publisher('/PETIT/resume_pathwrapper', Empty)
        pass
Пример #4
0
 def __init__(self):
     State.__init__(self, outcomes=['succeeded','aborted','preempted'])
     self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
     # Wait up to 60 seconds for the action server to become available
     self.move_base.wait_for_server(rospy.Duration(60))    
     
     rospy.loginfo("Connected to move_base action server")
Пример #5
0
 def __init__(self):
     self.object_detector = ObjectDetector()
     self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing',
                                                            TabletopCollisionMapProcessing)
     State.__init__(self, 
                    outcomes=['found_clusters','no_clusters'],
                    output_keys=['segmentation_result','cluster_information'])
Пример #6
0
 def __init__(self):
     self.policy = [
         #InfomaxAction.GRASP,
         #InfomaxAction.LIFT,
         #InfomaxAction.SHAKE_ROLL,
         #InfomaxAction.SHAKE_PITCH,
         InfomaxAction.DROP,
     ]
     
     self.action_to_outcome = {
         InfomaxAction.GRASP: 'grasp',
         InfomaxAction.LIFT: 'lift',
         InfomaxAction.SHAKE_ROLL: 'shake_roll',
         InfomaxAction.SHAKE_PITCH: 'shake_pitch',
         InfomaxAction.DROP: 'drop',
     }
     
     State.__init__(self, 
                    outcomes=['grasp',
                              'lift',
                              'shake_roll',
                              'shake_pitch',
                              'drop',
                              'aborted'],
                    input_keys=['action_index_in'],
                    output_keys=['infomax_action_id',
                                 'action_index_out'])
 def __init__(self, input_keys=['arm', 'action', 'lift']):
     action_uri = 'imgui_action'
     self.action_client = actionlib.SimpleActionClient(action_uri, IMGUIAction)
     rospy.loginfo("waiting for %s"%action_uri)
     self.action_client.wait_for_server()
     rospy.loginfo("%s found"%action_uri)
     State.__init__(self, outcomes=['succeeded', 'failed'], input_keys = input_keys)
 def __init__(self, room, timer):
     State.__init__(self, outcomes=['succeeded','aborted','preempted'])
     
     self.task = 'mop_floor'
     self.room = room
     self.timer = timer
     self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
Пример #9
0
    def __init__(self, room, timer):
        State.__init__(self, outcomes=["succeeded", "aborted", "preempted"])

        self.task = "mop_floor"
        self.room = room
        self.timer = timer
        self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist)
Пример #10
0
    def __init__(self, position, orientation):
        State.__init__(self, outcomes=['success'])

        #global pub
        # Get an action client
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.client.wait_for_server()

        # Define the goal
        self.goal = MoveBaseGoal()
        self.goal.target_pose.header.frame_id = '/map'
        self.goal.target_pose.pose.position.x = position[0]
        self.goal.target_pose.pose.position.y = position[1]
        self.goal.target_pose.pose.position.z = 0.0
        self.goal.target_pose.pose.orientation.x = orientation[0]
        self.goal.target_pose.pose.orientation.y = orientation[1]
        self.goal.target_pose.pose.orientation.z = orientation[2]
        self.goal.target_pose.pose.orientation.w = orientation[3]

        #publlish waypoint
        self.target_pose = PoseStamped()
        self.target_pose.header.frame_id = '/map'
        self.target_pose.pose.position.x = position[0]
        self.target_pose.pose.position.y = position[1]
        self.target_pose.pose.position.z = 0.0
	self.target_pose.pose.orientation.x = orientation[0]
	self.target_pose.pose.orientation.y = orientation[1]
	self.target_pose.pose.orientation.z = orientation[2]
	self.target_pose.pose.orientation.w = orientation[3]
    def __init__(self, topic, msg_type, max_checks = -1):
        State.__init__(self, outcomes=['invalid','valid','preempted'])        
        self._topic = topic
        self._msg_type = msg_type
        self._cond_cb = self.execute_cb
        self._max_checks = max_checks
        self._n_checks = 0

        self._trigger_cond = threading.Condition()

        self.bridge = CvBridge()
        rospy.sleep(1)
        self.task = 'Face Detection'
        cv.NamedWindow(self.task, cv.CV_WINDOW_NORMAL)
        # cv.ResizeWindow(self.task, 640, 480)
        # cv2.imshow(self.task, np.zeros((480,640), dtype = np.uint8))
        rospy.loginfo("waiting for images...")

        pkg_path = rospkg.RosPack().get_path('ros_project')

        cascade_1 = pkg_path+"/data/haar_detectors/haarcascade_frontalface_alt.xml"
        self.cascade1 = cv2.CascadeClassifier(cascade_1)
        cascade_2 = pkg_path+"/data/haar_detectors/haarcascade_frontalface_alt2.xml"
        self.cascade2 = cv2.CascadeClassifier(cascade_2)
        cascade_3 = pkg_path+"/data/haar_detectors/haarcascade_profileface.xml"
        self.cascade3 = cv2.CascadeClassifier(cascade_3)

        self.haar_params = dict(scaleFactor = 1.3,minNeighbors = 3,
                                flags = cv.CV_HAAR_DO_CANNY_PRUNING,
                                minSize = (30, 30),maxSize = (150, 150))
Пример #12
0
 def __init__(self, input_keys=["map", "unattached_objects", "attached_objects", "arm"]):
     action_uri = "imgui_action"
     self.action_client = actionlib.SimpleActionClient(action_uri, IMGUIAction)
     rospy.loginfo("waiting for %s" % action_uri)
     self.action_client.wait_for_server()
     rospy.loginfo("%s found" % action_uri)
     State.__init__(self, outcomes=["succeeded", "failed"], input_keys=input_keys)
Пример #13
0
 def __init__(self, room, timer):
     State.__init__(self, outcomes=['succeeded','aborted','preempted'])
     
     self.task = 'scrub_tub'
     self.room = room
     self.timer = timer
     self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
 def __init__(self, input_keys=['arm', 'object_id']):
     action_uri = 'pickup_im_object_action'
     self.pickup_client = actionlib.SimpleActionClient(action_uri, PickupIMObjectAction)
     rospy.loginfo("waiting for %s"%action_uri)
     self.pickup_client.wait_for_server()
     rospy.loginfo("%s found"%action_uri)
     State.__init__(self, outcomes=['succeeded', 'failed'], input_keys = input_keys)
Пример #15
0
 def __init__(self, question, resp=True):
     State.__init__(self, outcomes=['yes', 'no'])
     self.resp = 'yes' if resp else 'no'
     if resp:
         self.prompt = '%s [%s]|%s: ' % (question, 'y', 'n')
     else:
         self.prompt = '%s [%s]|%s: ' % (question, 'n', 'y')
Пример #16
0
 def __init__(self):
     State.__init__(self,
                    input_keys=['clusters',
                                'bounding_boxes',
                                'names',
                                'response'],
                    output_keys=['response'],
                    outcomes=['done'])
Пример #17
0
 def __init__(self, topic_name, topic_type, output_key_name='topic_data', timeout=None):
     State.__init__(self, outcomes=['succeeded', 'timeouted', 'preempted'], output_keys=[output_key_name])
     self._topic = topic_name
     self._topic_type = topic_type
     self._timeout = rospy.Duration(timeout) if timeout else None
     self._topic_data = None
     self._output_key_name = output_key_name
     self._mutex = threading.Lock()
 def __init__(self, smm):
     State.__init__(self,
                    outcomes=["clear_front",
                              "clear_left",
                              "clear_right",
                              "blocked",
                              "unknown"])
     self.smm = smm
Пример #19
0
 def __init__(self):
     self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
     self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction)
     self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction)
     self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
     self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
     
     State.__init__(self, output_keys=['action_index'], outcomes=['succeeded','aborted'])
Пример #20
0
 def __init__(self, square_topic='/nao_square', timeout=3.0, sleeptime=2.0):
     State.__init__(self, outcomes=['succeeded', 'aborted'], output_keys=['square'])
     self._topic = square_topic
     self._square = None
     self._squareBuffer = []
     self._countBuffer = 0
     self._timeout = rospy.Duration(timeout)
     self._sleeptime = sleeptime
 def __init__(self, dataset, object_id):
     State.__init__(self,
                    outcomes=['stored'],
                    input_keys=['bounding_boxes', 'clusters', 'mean',
                                'median', 'points'])
     base = roslib.packages.get_pkg_dir(PACKAGE)
     self.dataset = Dataset(join(base, 'common', 'data'), dataset)
     self.object_id = object_id
    def __init__(self, value):
        State.__init__(self, outcomes=['succeeded','aborted','preempted'])

	self.value = value
        self.distance_pub = rospy.Publisher('/PETIT/alpha_ardu', Int32)
        self.pause_pub = rospy.Publisher('/PETIT/pause_pathwrapper', Empty)
        self.resume_pub = rospy.Publisher('/PETIT/resume_pathwrapper', Empty)
        pass
Пример #23
0
    def __init__(self):
        State.__init__(self, outcomes=["succeeded", "aborted", "preempted"])
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self.pub = rospy.Publisher("speech", String)
        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move_base action server")
 def __init__(self, input_keys=['frame_id', 'x', 'y', 'theta', 'collision_aware']):
     move_base_uri = '/move_base'
     self.move_base_node_name = rospy.get_param('move_base_node_name', '/move_base_node')
     self.move_base_client = actionlib.SimpleActionClient(move_base_uri, mbm.MoveBaseAction)
     rospy.loginfo("waiting for move base server")
     self.move_base_client.wait_for_server()
     rospy.loginfo("move base server found")
     self.cmd_vel_pub = rospy.Publisher("base_controller/command", gm.Twist)
     State.__init__(self, outcomes = ['succeeded', 'failed', 'preempted', 'error'], input_keys = input_keys)
 def __init__(self, input_keys=['target_x', 'target_y', 'camera_info_topic']):
     action_uri = '/head_traj_controller/point_head_action'
     self.point_head_client = actionlib.SimpleActionClient(action_uri, PointHeadAction)
     rospy.loginfo("waiting for %s"%action_uri)
     self.point_head_client.wait_for_server()
     rospy.loginfo("%s found"%action_uri)
     State.__init__(self, outcomes=['succeeded', 'failed', 'preempted'], input_keys = input_keys)
     
     self.head_timeout = rospy.get_param('~head_timeout', PointHeadInImage.HEAD_TIMEOUT_DEFAULT)
Пример #26
0
    def __init__(self):
        State.__init__(self, outcomes=['succeeded'])
        self.ON_pub = rospy.Publisher('/MILIGHT/light3ON', Empty)
        self.ON2_pub = rospy.Publisher('/MILIGHT/light2ON', Empty)
	self.heatON_pub = rospy.Publisher('/HOME/showerHeatON', Empty)
        self.heatOFF_pub = rospy.Publisher('/HOME/showerHeatOFF', Empty)
        self.weather_pub = rospy.Publisher('/NESTOR/weather', Empty)
        self.french_pub = rospy.Publisher('/NESTOR/french_voice', String)
	self.music_pub = rospy.Publisher('/NESTOR/radio_jap', Empty)
 def __init__(self, input_keys=['position']):
     action_uri = '/torso_controller/position_joint_action'
     self.torso_client = actionlib.SimpleActionClient(action_uri, SingleJointPositionAction)
     rospy.loginfo("waiting for %s"%action_uri)
     self.torso_client.wait_for_server()
     rospy.loginfo("%s found"%action_uri)
     State.__init__(self, outcomes=['succeeded', 'failed', 'preempted'], input_keys = input_keys)
     
     self.torso_timeout = rospy.get_param('~torso_timeout', MoveTorso.TORSO_TIMEOUT_DEFAULT)
Пример #28
0
    def __init__(self, pool=None, max_tries=10):
        input_keys = []
        if not pool:
            input_keys = ['pool']

        State.__init__(self, outcomes=['succeeded'], output_keys=['selected_item'], input_keys=input_keys)
        self._pool = pool
        self._lenpool = len(pool) if pool else None
        self._lastSelection = None
        self._MAX_TRIES = max_tries
Пример #29
0
    def __init__(self):
        State.__init__(self, outcomes=['succeeded'])
        self.OFF1_pub = rospy.Publisher('/MILIGHT/light1OFF', Empty)
        self.brightness1_pub = rospy.Publisher('/MILIGHT/light1Brightness', Int32)
        self.OFF2_pub = rospy.Publisher('/MILIGHT/light2OFF', Empty)
        self.brightness2_pub = rospy.Publisher('/MILIGHT/light2Brightness', Int32)
        self.OFF3_pub = rospy.Publisher('/MILIGHT/light3OFF', Empty)
        self.brightness3_pub = rospy.Publisher('/MILIGHT/light3Brightness', Int32)
        self.heatOFF_pub = rospy.Publisher('/HOME/showerHeatOFF', Empty)
	self.french_pub = rospy.Publisher('/NESTOR/french_voice', String)
Пример #30
0
    def __init__(self):
        State.__init__(self, outcomes=['detect1','detect2','detect3','detect4','detect5','detect6'])
        self.taskname="doing the computer vision task"

        # initialize a subscriber of ar_pose_marker
        rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.my_listener)
        self.tag_ids = [0,1,2,3,4,5,6]

        self.marker_table_dictionary = {1:'detect1', 2:'detect2', 3:'detect3', 4:'detect4', 5:'detect5', 6:'detect6'}
        self.n_markers = 0;
        self.my_outcome = self.marker_table_dictionary[1];
Пример #31
0
 def __init__(self, ltmc):
     State.__init__(self,
                    output_keys=["location"],
                    outcomes=['succeeded', 'aborted'])
     self.ltmc = ltmc
Пример #32
0
 def __init__(self, angle):
     State.__init__(self, outcomes=['success'])
     sleep(1)
     self.angle = angle
Пример #33
0
 def __init__(self, amount):
     State.__init__(self, outcomes=["succeeded"])
     self.amount = amount
Пример #34
0
 def __init__(self):
     State.__init__(self,outcomes=['succeeded','aborted','error'],
                         input_keys=['gripper_mode'])
     self.obstacle_client = rospy.ServiceProxy('xm_obstacles',xm_Obstacles)
Пример #35
0
 def __init__(self):
     State.__init__(self,outcomes=['succeeded','aborted'])
Пример #36
0
 def __init__(self, distance):
     State.__init__(self, outcomes=['success'])
     self.distance = distance
Пример #37
0
 def __init__(self):
     State.__init__(self,outcomes=['succeeded','aborted','error'],
                         input_keys=['gripper_mode','arm_ps','objmode'])
    
     self.ik_client = rospy.ServiceProxy('xm_ik_solver',xm_SolveIK)
     self.tf_listener =tf.TransformListener()
Пример #38
0
    def __init__(self):
        State.__init__(self, outcomes=['stop', 'aborted'])

        self.target_client = rospy.ServiceProxy('xm_speech_meaning',
                                                xm_Speech_meaning)
Пример #39
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'error'],
                    input_keys=['arm_ps', 'arm_mode'])
     self.tf_listener = tf.TransformListener()
Пример #40
0
 def __init__(self, square, cam_pixel_to_point
              ):  # type: (ParkingSquare, CamPixelToPointServer) -> None
     State.__init__(self, outcomes=['ok'], input_keys=['green_shape'])
     self.feature_detector = FeatureDetector()
     self.cam_pixel_to_point = cam_pixel_to_point
     self.square = square
Пример #41
0
 def __init__(self, square):  # type: (ParkingSquare) -> None
     State.__init__(self, outcomes=['ok', 'shortcircuit'])
     self.square = square
Пример #42
0
 def __init__(self):
     State.__init__(self, outcomes=['succeeded', 'aborted', 'preempt'])
     self.cmd_vel = rospy.Publisher(
         '/mobile_base/mobile_base_controller/cmd_vel', Twist, queue_size=1)
     self.twist_xm = Twist()
Пример #43
0
 def __init__(self):
     State.__init__(self, outcomes=['succeeded', 'aborted'])
     self.tf_listener = tf.TransformListener()
Пример #44
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted'],
                    input_keys=['goal_id'],
                    output_keys=['goal_id'])
Пример #45
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted'],
                    output_keys=['target', 'name'])
Пример #46
0
 def __init__(self, radius_min=1, radius_max=3):
     State.__init__(self, outcomes=['succeeded'], output_keys=['x', 'y', 'yaw'])
     self.radius_min = radius_min
     self.radius_max = radius_max
Пример #47
0
 def __init__(self):
     State.__init__(self, 
     outcomes=["succeeded",'error'],
     input_keys=['rec'])
Пример #48
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted', 'error'],
                    input_keys=['arm_ps', 'arm_mode', 'objmode'])
     self.arm_client = rospy.ServiceProxy('arm_stack', xm_PickOrPlace)
Пример #49
0
 def __init__(self):
     State.__init__(self,outcomes = ['succeeded','error'])
Пример #50
0
 def __init__(self):
     State.__init__(self, outcomes=['success'])
Пример #51
0
 def __init__(self):
     State.__init__(self,outcomes=['succeeded','aborted','error','preempted'],
                     input_keys=['arm_mode'])
Пример #52
0
 def __init__(self, mode):
     State.__init__(self, ['succeeded', 'aborted', 'preempted'])
     self.mode = mode
     self.control_mode = ControllerMode()
Пример #53
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted', 'preempted'],
                    input_keys=['waypoints'],
                    output_keys=['waypoint_out'])
Пример #54
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted', 'error'],
                    input_keys=['name_list', 'name_id', 'name'])
Пример #55
0
 def __init__(self, pose):
     State.__init__(self,
                    outcomes=['1', '0'],
                    input_keys=['input'],
                    output_keys=[''])
     self.pose = pose
Пример #56
0
 def __init__(self):
     State.__init__(self,
                    outcomes=["succeeded", 'aborted', 'error'],
                    input_keys=['name_id', 'name_list', 'target_list'],
                    output_keys=['sentences'])
Пример #57
0
 def __init__(self, ltmc, visitable="visitable"):
     State.__init__(self,
                    output_keys=["location"],
                    outcomes=['succeeded', 'aborted'])
     self.ltmc = ltmc
     self.visitable = visitable
Пример #58
0
 def __init__(self):
     State.__init__(self, outcomes=['succeeded', 'aborted', 'error'])
     self.cv_client = rospy.ServiceProxy('get_object', xm_ObjectDetect)
Пример #59
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'error'],
                    input_keys=['target_name'],
                    io_keys=['target'])
Пример #60
0
 def __init__(self):
     State.__init__(self,
                    outcomes=['succeeded', 'aborted'],
                    input_keys=['arm_state'])