Python tf.TransformListener方法代码示例

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, camera_name, base_frame, table_height):
        """
        Initialize the instance

        :param camera_name: The camera name. One of (head_camera, right_hand_camera)
        :param base_frame: The frame for the robot base
        :param table_height: The table height with respect to base_frame
        """
        self.camera_name = camera_name
        self.base_frame = base_frame
        self.table_height = table_height

        self.image_queue = Queue.Queue()
        self.pinhole_camera_model = PinholeCameraModel()
        self.tf_listener = tf.TransformListener()

        camera_info_topic = "/io/internal_camera/{}/camera_info".format(camera_name)
        camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)

        self.pinhole_camera_model.fromCameraInfo(camera_info)

        cameras = intera_interface.Cameras()
        cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self):
        """
        Constructor.
        """
        self._state = dict()

        self._pub_pan = rospy.Publisher(
            '/robot/head/command_head_pan',
            HeadPanCommand,
            queue_size=10)

        state_topic = '/robot/head/head_state'
        self._sub_state = rospy.Subscriber(
            state_topic,
            HeadState,
            self._on_head_state)

        self._tf_listener = tf.TransformListener()

        intera_dataflow.wait_for(
            lambda: len(self._state) != 0,
            timeout=5.0,
            timeout_msg=("Failed to get current head state from %s" %
                         (state_topic,)),
        ) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, options, parent=None):
        super(ROSNode, self).__init__(parent)

        self.compiledMsgs = [m for m in dir(msgCF) if m[0]!="_" and m[-2:]=="CF"] # Messages that are previously auto-compiled and we can send
        if len(self.compiledMsgs)==0:
            rospy.logerror('Not Messages could be loaded. Please connect to the flie and press Compile Messages, then run rosmake')
        self.options = options
        # Publishers
        self.publishers   = {} #Generated publishers will go here
        self.pub_tf       = tf.TransformBroadcaster()

        # Subscribers
        self.sub_tf    = tf.TransformListener()
        self.sub_joy   = rospy.Subscriber("/cfjoy", cmdMSG, self.receiveJoystick)
        self.sub_baro = None # Defined later

        self.master = rosgraph.Master('/rostopic') 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self):

        # Tf listener (position + rpy) of end effector tool
        self.position = [0, 0, 0]
        self.rpy = [0, 0, 0]
        self.tf_listener = tf.TransformListener()

        # State publisher
        self.niryo_one_robot_state_publisher = rospy.Publisher(
            '/niryo_one/robot_state', RobotState, queue_size=5)

        # Get params from rosparams
        rate_tf_listener = rospy.get_param("/niryo_one/robot_state/rate_tf_listener")
        rate_publish_state = rospy.get_param("/niryo_one/robot_state/rate_publish_state")

        rospy.Timer(rospy.Duration(1.0 / rate_tf_listener), self.get_robot_pose)
        rospy.Timer(rospy.Duration(1.0 / rate_publish_state), self.publish_state)

        rospy.loginfo("Started Niryo One robot state publisher") 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, configs):
        self.configs = configs
        self.ROT_VEL = self.configs.BASE.MAX_ABS_TURN_SPEED
        self.LIN_VEL = self.configs.BASE.MAX_ABS_FWD_SPEED
        self.MAP_FRAME = self.configs.BASE.MAP_FRAME
        self.BASE_FRAME = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME
        self.point_idx = self.configs.BASE.TRACKED_POINT

        rospy.wait_for_service(self.configs.BASE.PLAN_TOPIC, timeout=3)
        try:
            self.plan_srv = rospy.ServiceProxy(self.configs.BASE.PLAN_TOPIC, GetPlan)
        except rospy.ServiceException:
            rospy.logerr(
                "Timed out waiting for the planning service. \
                    Make sure build_map in script and \
                           use_map in roslauch are set to the same value"
            )
        self.start_state = build_pose_msg(0, 0, 0, self.BASE_FRAME)
        self.tolerance = self.configs.BASE.PLAN_TOL
        self._transform_listener = tf.TransformListener() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, name, root,
            listener=None,
            broadcaster=None,
            history_length=0,
            offset=None):
        self.name = name
        self.transforms = {}
        self.root = root
        self.history = deque()
        self.history_length = history_length
        self.offset = offset


        if listener is not None:
            self.listener = listener
        else:
            self.listener = tf.TransformListener()
        if broadcaster is not None:
            self.broadcaster = broadcaster
        else:
            self.broadcaster = tf.TransformBroadcaster() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, root="/world", listener=None, max_dt=1.):
        self.objs = {}
        self.urdfs = {}
        self.frames = {}
        if listener is None:
            self.listener = tf.TransformListener()
        else:
            self.listener = listener
        self.root = root
        self.max_dt = max_dt

        rospy.wait_for_service('/get_planning_scene')

        self.co_pub = rospy.Publisher('collision_object',
                CollisionObject,
                queue_size=1000) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, mission, ignore_track_check, track_details = None):

        # ROS Subscribers
        self.sub_track = rospy.Subscriber('/fssim/track', Track, self.callback_track)

        self.received_track = False
        self.ignore_track_check = ignore_track_check
        self.mission = mission

        # If we find detailed track description we can set also initial position
        self.track_details = track_details

        # TF Initializations
        self.listener = tf.TransformListener()
        self.br = tf2_ros.StaticTransformBroadcaster()

        # Track Cones
        self.cones_left = []
        self.cones_right = [] 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self):
        """
        Constructor
        """
        rospy.init_node('spectator_camera', anonymous=True)
        self.listener = tf.TransformListener()
        self.role_name = rospy.get_param("/role_name", "ego_vehicle")
        self.camera_resolution_x = rospy.get_param("~resolution_x", 800)
        self.camera_resolution_y = rospy.get_param("~resolution_y", 600)
        self.camera_fov = rospy.get_param("~fov", 50)
        self.host = rospy.get_param('/carla/host', '127.0.0.1')
        self.port = rospy.get_param('/carla/port', '2000')
        self.timeout = rospy.get_param("/carla/timeout", 2)
        self.world = None
        self.pose = None
        self.camera_actor = None
        self.ego_vehicle = None
        rospy.Subscriber("/carla/{}/spectator_pose".format(self.role_name),
                         PoseStamped, self.pose_received) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self):
		rospy.init_node('DeadReckoning')
		
		self._VelocityCommandPublisher = rospy.Publisher("cmd_vel", Twist)
		self._TransformListener = tf.TransformListener()

		# wait for the listener to get the first transform message
		self._TransformListener.waitForTransform("/odom", "/base_link", rospy.Time(), rospy.Duration(4.0)) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, dt=None):
        self.dt = dt or 0.1
        self.sensor_names = ['quad_to_obj_pos', 'quad_to_obj_rot', 'image']

        # self.quad_to_obj_pos = np.zeros(3)
        # self.quad_to_obj_rot = np.array([0, 0, 0, 1])
        # self.image = np.zeros((32, 32), dtype=np.uint8)
        # self.quad_pos = np.zeros(3)
        # self.quad_rot = np.array([0, 0, 0, 1])
        self.quad_to_obj_pos = None
        self.quad_to_obj_rot = None
        self.image = None
        self.quad_pos = None
        self.quad_rot = None

        self.camera_control_pub = rospy.Publisher("/bebop/camera_control", geometry_msgs.Twist, queue_size=1, latch=True)
        self.cmd_vel_pub = rospy.Publisher("/vservo/cmd_vel", geometry_msgs.Twist, queue_size=1)

        # set camera's tilt angle to -35 degrees
        twist_msg = geometry_msgs.Twist(
            linear=geometry_msgs.Point(0., 0., 0.),
            angular=geometry_msgs.Point(0., -35., 0.)
        )
        self.camera_control_pub.publish(twist_msg)

        self.listener = tf.TransformListener()
        self.image_sub = rospy.Subscriber("/bebop/image_raw", sensor_msgs.Image, callback=self._image_callback)
        self.cv_bridge = cv_bridge.CvBridge()

        self._action_space = TranslationAxisAngleSpace(-np.ones(4), np.ones(4), axis=np.array([0, 0, 1]))

        self.rate = rospy.Rate(1.0 / self.dt)

        rospy.sleep(1) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self):
        
        rospy.init_node('flieLocaliser') 
        
        self.maxDepth = 4.5
        self.bg_thresh = 0.45
        self.kernel = np.ones((3, 3), np.uint8)
        self.centerX = 319.5
        self.centerY = 239.5
        self.depthFocalLength = 525

        # CV stuff
        self.bridge = CvBridge()

           
        # Subscribe
        #self.sub_rgb = rospy.Subscriber("/camera/rgb/image_raw", ImageMSG, self.new_rgb_data)
        self.sub_depth = rospy.Subscriber("/camera/depth_registered/image_raw", ImageMSG, self.new_depth_data)
        self.pub_depth = rospy.Publisher("/camera/detector", ImageMSG)
        self.sub_tf = tf.TransformListener()

        # Publish
        self.pub_tf = tf.TransformBroadcaster()

        # Members
        self.depth = None
        self.show = None
        self.counter = 30*4

        self.acc = np.zeros((480,640,self.counter), dtype=np.float32)


        
        # Run ROS node
        rospy.loginfo('Node Started')
        try:
            rospy.spin()
        except KeyboardInterrupt:
            print "Shutting down"
            cv2.destroyAllWindows() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self,options=None):     
        self.options = options
           
        self.joy_scale = [-1,1,-1,1,1] #RPYTY
        self.trim_roll = 0
        self.trim_pitch = 0
        self.max_angle = 30
        self.max_yawangle = 200
        
        
        self.max_thrust = 80.
        self.min_thrust = 25.
        self.max_thrust_raw = percentageToThrust(self.max_thrust)
        self.min_thrust_raw = percentageToThrust(self.min_thrust)       
        self.old_thurst_raw = 0
        
        self.slew_limit = 45
        self.slew_rate = 30
        self.slew_limit_raw = percentageToThrust(self.slew_limit)            
        self.slew_rate_raw = percentageToThrust(self.slew_rate)   
        
        self.dynserver = None
        self.prev_cmd = None
        self.curr_cmd = None
        self.yaw_joy = True
        
        self.sub_joy   = rospy.Subscriber("/joy", JoyMSG, self.new_joydata)
        self.sub_tf    = tf.TransformListener()         
        self.pub_tf    = tf.TransformBroadcaster()    
        self.pub_cfJoy = rospy.Publisher("/cfjoy", CFJoyMSG)
        
        # Dynserver               
        
        self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self,point_cloud_topic,detection_frame,destination_frame):
        """
        Initializes a Block Detector

        @param point_cloud_topic the name of the point-cloud to read from
        @param detection_frame the TF frame of the camera
        @param destination_frame the TF frame of block_in_world
        """

        super(PerceptionModule, self).__init__()

        self.point_cloud_topic = point_cloud_topic
        self.detection_frame = detection_frame
        self.destination_frame = destination_frame
        self.listener = tf.TransformListener() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformListener [as 别名]
def __init__(self, kinbody_path, detection_frame, world_frame,
                 service_namespace=None):
        """
        This initializes a simtrack detector.
        
        @param kinbody_path The path to the folder where kinbodies are stored
        @param detection_frame The TF frame of the camera
        @param world_frame The desired world TF frame
        @param service_namespace The namespace for the simtrack service (default: /simtrack)
        """
        import rospy
        import tf
        import tf.transformations as transformations
        # Initialize a new ros node if one has not already been created
        try:
            rospy.init_node('simtrack_detector', anonymous=True)
        except rospy.exceptions.ROSException:
            pass
            
        #For getting transforms in world frame
        if detection_frame is not None and world_frame is not None:
            self.listener = tf.TransformListener()
        else:
            self.listener = None
                
        if service_namespace is None:
            service_namespace='/simtrack'

        self.detection_frame = detection_frame
        self.world_frame = world_frame
        self.service_namespace = service_namespace
            
        self.kinbody_path = kinbody_path

        # A map of known objects that can be queries
        self.kinbody_to_query_map = {'fuze_bottle': 'fuze_bottle_visual',
                                     'pop_tarts': 'pop_tarts_visual'}
        self.query_to_kinbody_map = {'fuze_bottle_visual': 'fuze_bottle',
                                     'pop_tarts_visual': 'pop_tarts'}