def __init__(self, hand, force_threshold=1.5): '''Constructor''' super(WaitForForceState, self).__init__(outcomes=['success', 'failure']) self._action_name = rospy.get_name() self._prefix = "meka_roscontrol" self._client = {} self._movement_finished = {} self.force_variation = {} self.force_bias = {} self.previous_force = {} self._r = rospy.Rate(100) self._threshold = force_threshold # store latest planning scene self.current_planning_scene = None self.force_variation['left_arm'] = numpy.array([0.0, 0.0, 0.0]) self.force_variation['right_arm'] = numpy.array([0.0, 0.0, 0.0]) self.force_bias['left_arm'] = numpy.array([0.0, 0.0, 0.0]) self.force_bias['right_arm'] = numpy.array([0.0, 0.0, 0.0]) self.previous_force['left_arm'] = numpy.array([0.0, 0.0, 0.0]) self.previous_force['right_arm'] = numpy.array([0.0, 0.0, 0.0]) self._current_val = 0 self._group = hand + '_arm' self.sub_left = ProxySubscriberCached( {'/meka_ros_pub/m3loadx6_ma30_l0/wrench': WrenchStamped}) self.sub_right = ProxySubscriberCached( {'/meka_ros_pub/m3loadx6_ma29_l0/wrench': WrenchStamped})
def test_publish_subscribe(self): t1 = '/pubsub_1' t2 = '/pubsub_2' pub = ProxyPublisher({t1: String}) pub = ProxyPublisher({t2: String}, _latch=True) sub = ProxySubscriberCached({t1: String}) self.assertTrue(pub.is_available(t1)) self.assertTrue(pub.wait_for_any(t1)) self.assertFalse(pub.wait_for_any(t2)) pub.publish(t1, String('1')) pub.publish(t2, String('2')) rospy.sleep( .5) # make sure latched message is sent before subscriber is added sub = ProxySubscriberCached({t2: String}) rospy.sleep( .5) # make sure latched message can be received before checking self.assertTrue(sub.has_msg(t1)) self.assertEqual(sub.get_last_msg(t1).data, '1') sub.remove_last_msg(t1) self.assertFalse(sub.has_msg(t1)) self.assertIsNone(sub.get_last_msg(t1)) self.assertTrue(sub.has_msg(t2)) self.assertEqual(sub.get_last_msg(t2).data, '2')
def __init__( self, output_topic="/bounding_box_2d_monitor", rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info", yolo_yaml="yolov3.yaml", save_image=True, z_offset=0.1, MEMO="/spacoty/get_dataset"): super(hsr_SpaCoTyGetDataset, self).__init__(outcomes=['completed', 'failed'], input_keys=['save_folder']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = { "img": "bounding_box_2d", "sw": "/spacoty/get_dataset", "wrd": "/spacoty/place_name" } self._output_topic = output_topic self._depth_topic = depth_topic self._rgb_topic = rgb_topic self._camera_info_topic = camera_info_topic self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/" self._save_image = save_image self.camera_info = None self.rgb_image = None self.depth_image = None self.z_offset = z_offset # get object labels # self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml # obj_yaml = rosparam.load_file(self._yolo_yaml_path) # self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"] self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml obj_yaml = rosparam.load_file(self._yolo_yaml_path) self.obj_class = obj_yaml[0][0].values() # pass required clients as dict (topic: type) self._client = ProxyActionClient( {self._topic["img"]: hsr_common.msg.BoundingBox2DAction}) self.bridge = cv_bridge.CvBridge() # Subscriber self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool}) self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String}) # define the dimension self._data_dim = {"pose": 3, "object": len(self.obj_class), "word": 0} # It may happen that the action client fails to send the action goal. self._error = False self._get_image_error = False
def __init__(self): '''Constructor''' super(GetClosestEntity, self).__init__(outcomes=['done', 'not_found'], input_keys=['entityList'], output_keys=['output_entity']) self._sub = ProxySubscriberCached({'/entities': Entities}) self._subpos = ProxySubscriberCached({'/robot_pose': Pose}) self.mypose = None
def __init__(self): # See example_state.py for basic explanations. super(WonderlandAddUpdatePeople, self).__init__(outcomes=['done']) self.url = "http://wonderland:8000/api/people/" self._sub = ProxySubscriberCached({'/entities': Entities}) self._topic = "/robot_pose" self._subscriber_pos = ProxySubscriberCached({self._topic: Pose}) self.my_pose = None self.message = None
def __init__(self): ''' Constructor ''' super(GetEmptyChair, self).__init__(outcomes=['done', 'nothing_found'], output_keys=['output_entity']) self._sub = ProxySubscriberCached({'/entities': Entities}) self._subpos = ProxySubscriberCached({'/robot_pose': Pose}) self.mypose = None self.message = None
def __init__(self, bumper_topic='mobile_base/events/bumper', cliff_topic='mobile_base/events/cliff'): super(TurtlebotStatusState, self).__init__(outcomes=['bumper', 'cliff']) self._bumper_topic = bumper_topic self._cliff_topic = cliff_topic self._bumper_sub = ProxySubscriberCached( {self._bumper_topic: BumperEvent}) self._cliff_sub = ProxySubscriberCached( {self._cliff_topic: CliffEvent}) self._return = None # Handle return in case outcome is blocked by low autonomy
def __init__(self): """ Constructor """ super(WaitForOpenDoorState, self).__init__(outcomes=['done']) self._topic = '/laser/srd_front/scan' self._sub = ProxySubscriberCached({self._topic: LaserScan})
def __init__(self): '''Constructor''' super(GetLaserscanState, self).__init__(outcomes=['done'], output_keys=['laserscan']) self._scan_topic = "/multisense/lidar_scan" self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self.ref_frame = userdata.ref_frame self._topic = userdata.camera_topic self._camera_frame = userdata.camera_frame self._connected = False self._failed = False self._start_time = rospy.get_rostime() # Subscribe to the topic for the logical camera (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn( 'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic))) # Get transform between camera and robot_base try: self._transform = self._tf_buffer.lookup_transform( self.ref_frame, self._camera_frame, rospy.Time(0), rospy.Duration(1.0)) except Exception as e: Logger.logwarn('Could not transform pose: ' + str(e)) self._failed = True
def __init__(self, sentence, input_keys=[], emotion=0, block=True): """Constructor""" super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys) if not (isinstance(sentence, types.LambdaType) and sentence.__name__ == "<lambda>") and len(input_keys) is not 0: raise ValueError( "In state " + type(self).__name__ + " saying \"" + sentence + "\", you need to define a lambda function for sentence.") # Get parameters self.msg = say() self.sentence = sentence self.emotion = UInt8() self.emotion.data = emotion self.block = block # Set topics self.emotion_topic = "sara_face/Emotion" self.say_topic = "/say" self.sayServiceTopic = "/wm_say" self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8}) # Prepare wm_tts if self.block: self.sayClient = ProxyServiceCaller( {self.sayServiceTopic: say_service}) else: self.sayPub = ProxyPublisher({self.say_topic: say}) # Initialise the face sub self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8}) self.lastEmotion = None
def __init__(self): """Constructor""" super(KeepLookingAt, self).__init__(outcomes=['failed'], input_keys=['ID']) self.entities_topic = "/entities" self._sub = ProxySubscriberCached({self.entities_topic: Entities}) self.pubp = rospy.Publisher("/sara_head_pitch_controller/command", Float64, queue_size=1) self.puby = rospy.Publisher("/sara_head_yaw_controller/command", Float64, queue_size=1) # Reference from and reference to self.service = get_directionRequest() self.service.reference = "head_xtion_link" self.service.origine = "base_link" self.Entity = None self.serviceName = '/get_direction' Logger.loginfo('waiting for service '+str(self.serviceName)) self.serv = rospy.ServiceProxy(self.serviceName, get_direction) self._active = True try: self.serv.wait_for_service(1) except: self._active = False
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. for i in [0, 1, 2, 3, 4, 5]: self._topic = "/ariac/logical_camera_" + str(i) self._start_time = rospy.Time.now() (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) elapsed = rospy.get_rostime() - self._start_time while (elapsed.to_sec() < self._wait): elapsed = rospy.get_rostime() - self._start_time if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for model in message.models: if model.type == userdata.part_type: userdata.bin = "bingr" + str(i) + "PreGrasp" userdata.camera_topic = "/ariac/logical_camera_" + str( i) userdata.camera_frame = "logical_camera_" + str( i) + "_frame" Logger.loginfo("bingr" + str(i) + "PreGrasp") userdata.ref_frame = "torso_main" return 'continue' Logger.loginfo("part_type not found") return 'failed'
def __init__(self, *args, **kwargs): super(RosState, self).__init__(*args, **kwargs) self._rate = rospy.Rate(10) self._is_controlled = False self._pub = ProxyPublisher() self._sub = ProxySubscriberCached()
def __init__(self, target_time, velocity, rotation_rate, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state'): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreateTimedTwistState, self).__init__(outcomes=['done', 'failed']) # Store state parameter for later use. self._target_time = rospy.Duration(target_time) self._twist = TwistStamped() self._twist.twist.linear.x = velocity self._twist.twist.angular.z = rotation_rate # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._done = None # Track the outcome so we can detect if transition is blocked self._sensor_topic = sensor_topic self._cmd_topic = cmd_topic self._sensor_sub = ProxySubscriberCached( {self._sensor_topic: CreateSensorState}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
def __init__(self): '''Constructor''' super(GetCameraImageState, self).__init__(outcomes=['done'], output_keys=['camera_img']) self._img_topic = "/multisense/camera/left/image_rect_color" self._sub = ProxySubscriberCached({self._img_topic: Image})
def __init__(self, target_time, distance, cmd_topic='/cmd_vel', odometry_topic='/odom'): super(MoveDistanceState, self).__init__(outcomes=['done', 'failed']) self._target_time = target_time self._distance = distance self._velocity = (distance / target_time) self._twist = TwistStamped() self._twist.twist.linear.x = self._velocity self._twist.twist.angular.z = 0 # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._return = None # Track the outcome so we can detect if transition is blocked self._cmd_topic = cmd_topic self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) self._odom_topic = odometry_topic self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._starting_odom = None
def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Arm_Control_State_GR, self).__init__(outcomes = ["continue", "failed"]) self._pub = ProxyPublisher({"sim2real/reset_status": Int32}) self._sub = ProxySubscriberCached({"sim2real/reset": Int32})
def __init__(self, ref_frame, camera_topic, camera_frame): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(DetectPartCameraState, self).__init__(outcomes=['continue', 'failed'], output_keys=['pose']) self.ref_frame = ref_frame self._topic = camera_topic self._camera_frame = camera_frame self._connected = False self._failed = False # tf to transfor the object pose self._tf_buffer = tf2_ros.Buffer( rospy.Duration(10.0)) #tf buffer length self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) # Subscribe to the topic for the logical camera (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn( 'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def __init__(self, leap_motion_topic, exit_states, pose_topic='', parameters=[2.0, 0.02, 0.2]): super(LeapMotionMonitor, self).__init__( outcomes=['no_object', 'still_object', 'moving_object'], output_keys=['pose']) # store state parameter for later use. self._leap_topic = leap_motion_topic self._pose_topic = pose_topic self._exit_states = exit_states self._detecton_period = parameters[0] self._position_tolerance = parameters[1] self._orientation_tolerance = parameters[2] # setup proxies self._leap_subscriber = ProxySubscriberCached( {self._leap_topic: leapros}) if self._pose_topic: self._pose_publisher = ProxyPublisher( {self._pose_topic: PoseStamped}) else: self._pose_publisher = None # pose buffers self._pose = None self._pose_prev = None self._pose_averaged = None self._nonstillness_stamp = None
def __init__(self, topic = 'move_base_simple/goal'): """Constructor""" super(GetPoseState, self).__init__(outcomes=['done'], output_keys=['goal']) self._topic = topic self._sub = ProxySubscriberCached({self._topic: PoseStamped}) self._goal_pose = None
def __init__(self, frontality_level, distance_max=10): ''' Constructor ''' super(list_entities_by_name, self).__init__(outcomes=['found', 'none_found'], output_keys=['entity_list', 'number'], input_keys=['name']) self._sub = ProxySubscriberCached({'/entities': Entities}) self._topic = "/robot_pose" self._subpos = ProxySubscriberCached({self._topic: Pose}) self.frontality_level = frontality_level self.mypose = None self.message = None self.distance_max = distance_max
def __init__(self, distance=1, ReplanPeriod=0.5): """Constructor""" super(SaraFollow, self).__init__(outcomes=['failed'], input_keys=['ID']) # Prepare the action client for move_base self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) # Initialise the subscribers self.entities_topic = "/entities" self._pose_topic = "/robot_pose" self._sub = ProxySubscriberCached({ self._pose_topic: Pose, self.entities_topic: Entities }) # Get the parameters self.targetDistance = distance self.ReplanPeriod = ReplanPeriod # Initialise the status variables self.MyPose = None self.lastPlanTime = 0
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0): # Process supplied pose list. Note it must be processed befor superclass constructor is called, # beacuse it changes 'outcomes' list due to side effect. # check if 'unknown' outcome is present if 'unknown' not in outcomes: raise RuntimeError, '"unknown" should presents in state outcomes' # remove 'unknown' while preserving items order poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ] # Load poses from paramere server. Pose is loaded in form dict(joint name -> position), # but they are stored as a list of tuples (pose_name, pose_dict) to preserve order. self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ] # Call superclass constructor. super(CheckJointState, self).__init__(outcomes = outcomes) # Subscribe to the joint topic. self._joint_topic = joint_topic self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState }) # Store topic parameter for later use. self._tolerance = tolerance self._timeout = Duration.from_sec(timeout) # timestamp self._timestamp = None
def __init__(self): super(hsr_SpeechRecognitionByHeadMic,self).__init__(outcomes=['recognition'],output_keys=['recognition']) self._topic = "/hsrb/voice/text" self.Speech2Text_CB = ProxySubscriberCached({self._topic: RecognitionResult}) #self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction) # LED self.goal = ledGoal() p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True)
def __init__(self, topic): super(StorePointcloudState, self).__init__(outcomes = ['done'], output_keys = ['pointcloud']) self._sub = ProxySubscriberCached({topic: PointCloud2}) self._pcl_topic = topic
def test_lockable_state(self): state = self._create() fb_topic = 'flexbe/command_feedback' sub = ProxySubscriberCached({fb_topic: CommandFeedback}) rospy.sleep(0.2) # wait for pub/sub # lock and unlock as commanded, return outcome after unlock state._sub._callback(String('/subject'), 'flexbe/command/lock') state.result = 'done' outcome = self._execute(state) self.assertIsNone(outcome) self.assertTrue(state._locked) self.assertMessage( sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) state.result = None state._sub._callback(String('/subject'), 'flexbe/command/unlock') outcome = self._execute(state) self.assertEqual(outcome, 'done') self.assertMessage( sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) # lock and unlock without target state._sub._callback(String(''), 'flexbe/command/lock') state.result = 'done' outcome = self._execute(state) self.assertIsNone(outcome) self.assertMessage( sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) state._sub._callback(String(''), 'flexbe/command/unlock') outcome = self._execute(state) self.assertEqual(outcome, 'done') self.assertMessage( sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) # reject invalid lock command state._sub._callback(String('/invalid'), 'flexbe/command/lock') outcome = self._execute(state) self.assertEqual(outcome, 'done') self.assertMessage( sub, fb_topic, CommandFeedback(command='lock', args=['/invalid', ''])) # reject generic unlock command when not locked state._sub._callback(String(''), 'flexbe/command/unlock') self._execute(state) self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['', ''])) # do not transition out of locked container state.parent._locked = True outcome = self._execute(state) self.assertIsNone(outcome) state.parent._locked = False state.result = None outcome = self._execute(state) self.assertEqual(outcome, 'done')
def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._joint_config = userdata.joint_values self._joint_names = userdata.joint_names self._sub = ProxySubscriberCached({self._position_topic: JointState}) self._current_position = [] self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) # Action Initialization action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group action_goal.request.allowed_planning_time = 1.0 goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append( JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i], weight=1.0)) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True
def __init__(self): ''' Constructor ''' super(GetPositionToPlaceOnTable, self).__init__(outcomes=['done', 'not_found'], input_keys=['distanceFromEdge'], output_keys=['position']) self.planeTopic = "/segment_table/plane" self.planeSub = ProxySubscriberCached({self.planeTopic: PointCloud2}) self.robotposeTopic = "/robot_pose" self.robotposeSub = ProxySubscriberCached({self.robotposeTopic: Pose}) self.plane = None
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. # One of the outcomes declared above. for i in [1,2,3,4,5,6]: self._topic = "/ariac/logical_camera_stock"+str(i) self._start_time = rospy.Time.now() (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) elapsed = rospy.get_rostime() - self._start_time; while (elapsed.to_sec() < self._wait): elapsed = rospy.get_rostime() - self._start_time; if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for model in message.models: if model.type == userdata.part_type: if userdata.arm_id == "Right_Arm": userdata.gantry_pos = "Gantry_Part"+str(i)+"_R" elif userdata.arm_id == "Left_Arm": userdata.gantry_pos = "Gantry_Part"+str(i)+"_L" userdata.camera_topic = "/ariac/logical_camera_stock"+str(i) userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame" Logger.loginfo("Gantry_Part"+str(i)) if i < 5: return 'bin' else: return 'shelf' Logger.loginfo("part_type not found") return 'not_found'