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
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
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")
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'])
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)
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)
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))
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)
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)
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')
def __init__(self):
State.__init__(self,
input_keys=['clusters',
'bounding_boxes',
'names',
'response'],
output_keys=['response'],
outcomes=['done'])
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
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'])
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
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)
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)
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
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)
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];
def __init__(self, ltmc):
State.__init__(self,
output_keys=["location"],
outcomes=['succeeded', 'aborted'])
self.ltmc = ltmc
def __init__(self, angle):
State.__init__(self, outcomes=['success'])
sleep(1)
self.angle = angle
def __init__(self, amount):
State.__init__(self, outcomes=["succeeded"])
self.amount = amount
def __init__(self):
State.__init__(self,outcomes=['succeeded','aborted','error'],
input_keys=['gripper_mode'])
self.obstacle_client = rospy.ServiceProxy('xm_obstacles',xm_Obstacles)
def __init__(self):
State.__init__(self,outcomes=['succeeded','aborted'])
def __init__(self, distance):
State.__init__(self, outcomes=['success'])
self.distance = distance
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()
def __init__(self):
State.__init__(self, outcomes=['stop', 'aborted'])
self.target_client = rospy.ServiceProxy('xm_speech_meaning',
xm_Speech_meaning)
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'error'],
input_keys=['arm_ps', 'arm_mode'])
self.tf_listener = tf.TransformListener()
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
def __init__(self, square): # type: (ParkingSquare) -> None
State.__init__(self, outcomes=['ok', 'shortcircuit'])
self.square = square
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()
def __init__(self):
State.__init__(self, outcomes=['succeeded', 'aborted'])
self.tf_listener = tf.TransformListener()
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted'],
input_keys=['goal_id'],
output_keys=['goal_id'])
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted'],
output_keys=['target', 'name'])
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
def __init__(self):
State.__init__(self,
outcomes=["succeeded",'error'],
input_keys=['rec'])
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)
def __init__(self):
State.__init__(self,outcomes = ['succeeded','error'])
def __init__(self):
State.__init__(self, outcomes=['success'])
def __init__(self):
State.__init__(self,outcomes=['succeeded','aborted','error','preempted'],
input_keys=['arm_mode'])
def __init__(self, mode):
State.__init__(self, ['succeeded', 'aborted', 'preempted'])
self.mode = mode
self.control_mode = ControllerMode()
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['waypoints'],
output_keys=['waypoint_out'])
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['name_list', 'name_id', 'name'])
def __init__(self, pose):
State.__init__(self,
outcomes=['1', '0'],
input_keys=['input'],
output_keys=[''])
self.pose = pose
def __init__(self):
State.__init__(self,
outcomes=["succeeded", 'aborted', 'error'],
input_keys=['name_id', 'name_list', 'target_list'],
output_keys=['sentences'])
def __init__(self, ltmc, visitable="visitable"):
State.__init__(self,
output_keys=["location"],
outcomes=['succeeded', 'aborted'])
self.ltmc = ltmc
self.visitable = visitable
def __init__(self):
State.__init__(self, outcomes=['succeeded', 'aborted', 'error'])
self.cv_client = rospy.ServiceProxy('get_object', xm_ObjectDetect)
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'error'],
input_keys=['target_name'],
io_keys=['target'])
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted'],
input_keys=['arm_state'])
