Python tf.TransformBroadcaster方法代码示例

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [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 TransformBroadcaster [as 别名]
def __init__(self, env, child_frame, extents,
                 pose=None, period=0.05, parent_frame='world'):
        self.child_frame = child_frame
        self.parent_frame = parent_frame
        self.period = period

        with env:
            self.body = openravepy.RaveCreateKinBody(env, '')
            aabbs = numpy.concatenate(([0., 0., 0.], extents)).reshape((1, 6))
            self.body.InitFromBoxes(aabbs, True)
            self.body.SetName('frame:' + child_frame)

            if pose is not None:
                self.body.SetTransform(pose)

            env.Add(self.body, True)

        import tf
        self.broadcaster = tf.TransformBroadcaster()

        self.update() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self):
        """Initialize tracker.
        """
        self._read_configuration()

        self.estimates = {}
        self.estimate_times = {}
        self.ikf_prev_outlier_flags = {}
        self.ikf_outlier_counts = {}
        self.outlier_thresholds = {}

        rospy.loginfo("Receiving raw multi-range messages from: {}".format(self.uwb_multi_range_topic))
        rospy.loginfo("Publishing tracker messages to {}".format(self.uwb_tracker_topic))
        rospy.loginfo("Publishing tracker transform as {} -> {}".format(self.tracker_frame, self.target_frame))

        # ROS publishers and subscribers
        self.tracker_frame = self.tracker_frame
        self.target_frame = self.target_frame
        self.uwb_pub = rospy.Publisher(self.uwb_tracker_topic, uwb.msg.UWBTracker, queue_size=1)
        self.tf_broadcaster = tf.TransformBroadcaster()
        self.uwb_multi_range_sub = rospy.Subscriber(self.uwb_multi_range_topic, uwb.msg.UWBMultiRangeWithOffsets,
                                                    self.handle_multi_range_message) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [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 TransformBroadcaster [as 别名]
def handle_turtle_pose(msg):
    br = tf.TransformBroadcaster()
    LaserScan laser_tf 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def handle_turtle_pose(msg):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.origin.position.x, msg.origin.position.y, msg.origin.position.z), (msg.origin.orientation.x, msg.origin.orientation.y, msg.origin.orientation.z, msg.origin.orientation.w), rospy.Time.now(), "odom", "map") 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 9600))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [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 TransformBroadcaster [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 TransformBroadcaster [as 别名]
def __init__(self):

      # initialize ROS node and transform publisher
      rospy.init_node('crazyflie_detector', anonymous=True)
      self.pub_tf = tf.TransformBroadcaster()

      self.rate = rospy.Rate(50.0)                      # publish transform at 50 Hz

      # initialize values for crazyflie location on Kinect v2 image
      self.cf_u = 0                        # u is pixels left(0) to right(+)
      self.cf_v = 0                        # v is pixels top(0) to bottom(+)
      self.cf_d = 0                        # d is distance camera(0) to crazyflie(+) from depth image
      self.last_d = 0                      # last non-zero depth measurement

      # crazyflie orientation to Kinect v2 image (Euler)
      self.r = -1.5708
      self.p = 0
      self.y = -3.1415

      # Convert image from a ROS image message to a CV image
      self.bridge = CvBridge()

      cv2.namedWindow("KinectV2", 1)

      # Wait for the camera_info topic to become available
      rospy.wait_for_message('/kinect2/qhd/camera_info', CameraInfo)

      # Subscribe to Kinect v2 sd camera_info to get image frame height and width
      rospy.Subscriber('/kinect2/qhd/camera_info', CameraInfo, self.camera_data, queue_size=1)

      # Subscribe to registered color and depth images
      rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1)
      rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1)

      self.rate.sleep()                        # suspend until next cycle

   # This callback function sets parameters regarding the camera. 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self, file_name=None):
        """
        Note that file_name is not absolute path. All calibration files are
        assumed to be in ~/.robot/
        """
        params = {}
        if not file_name:
            if os.path.exists(get_abs_path(CALIBRATED_CONFIG_FNAME)):
                file_name = CALIBRATED_CONFIG_FNAME
            else:
                rospy.logerr("Using default camera calibration")
                rospy.logerr("For better performance, calibrate your system.")
                file_name = DEFAULT_CONFIG_FNAME
        file_path = get_abs_path(file_name)
        if os.path.exists(file_path):
            rospy.loginfo("Loading transforms from {:}".format(file_path))
            with open(file_path, "r") as f:
                params = json.load(f)
            self.params = params
            rospy.logwarn("Will publish: ")
            for t in self.params.values():
                rospy.logwarn("  {} to {}".format(t["from"], t["to"]))
            self.br = tf.TransformBroadcaster()
            self.publish_transforms()
        else:
            rospy.logerr("Unable to find calibration config file {}".format(file_path))
            sys.exit(0) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self):
        self.seq = 0
        self.tf_pub = tf.TransformBroadcaster()
        self.orange1 = (0.641782207489,
                  -0.224464386702,
                  -0.363829042912)
        self.orange2 = (0.69,
                  -0.31,
                  -0.363829042912)
        self.orange3 = (0.68,
                  -0.10,
                  -0.363829042912) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self):
        # ArUco data -- we're using 6x6 ArUco images
        self._aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
        self._aruco_parameters = cv2.aruco.DetectorParameters_create()

        # Initialize ROS
        rospy.init_node('detect_aruco_node', anonymous=False)

        # ROS publishers
        self._image_pub = rospy.Publisher('image_marked', Image, queue_size=10)
        self._rviz_markers_pub = rospy.Publisher('rviz_markers', MarkerArray, queue_size=10)

        # ROS OpenCV bridge
        self._cv_bridge = CvBridge()

        # ROS transform managers
        self._tf_listener = tf.TransformListener()
        self._tf_broadcaster = tf.TransformBroadcaster()

        # Get base_link => camera_frame transform
        self._tf_listener.waitForTransform("base_link", "camera_frame", rospy.Time(), rospy.Duration(4))
        self._Tcb = tf_to_matrix(self._tf_listener.lookupTransform("base_link", "camera_frame", rospy.Time()))

        # Now that we're initialized, set up subscriptions and spin
        rospy.Subscriber("image_raw", Image, self.image_callback)
        rospy.spin() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def pub_position_struct(xx,pub_pose,cnt):
    pose = Pose()
    pose.position.x,pose.position.y,pose.position.z=xx[:3]

    
    orientation = tf.transformations.quaternion_from_euler(xx[3], xx[4], xx[5], 'szxy')
    pose.orientation.x = orientation[0]
    pose.orientation.y = orientation[1]
    pose.orientation.z = orientation[2]
    pose.orientation.w = orientation[3]

    # Broadcast transform
    #br = tf.TransformBroadcaster()
    #br.sendTransform(xx[:3], orientation, rospy.Time.now(), "parag_rov", "world")
    pub_pose.publish(pose) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import TransformBroadcaster [as 别名]
def __init__(self):
        """
        """
        self.gazebo_trays = []
        self.gazebo_blocks = []

        self.trays = []
        self.blocks = []

        self.tf_broacaster = tf.TransformBroadcaster()
        self.tf_listener = tf.TransformListener()

        # initial simulated implementation
        pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.simulated_link_state_callback, queue_size=10)

        self.gazebo_world_to_ros_transform = None
        self.original_blocks_poses_ = None
        self.mutex = RLock()

        TABLE_HEIGHT = -0.12
        self.head_camera_helper = CameraHelper("head_camera", "base", TABLE_HEIGHT)
        self.bridge = CvBridge()
        self.block_pose_estimation_head_camera = None
        self.table = Table()

        self.hand_camera_helper = CameraHelper("right_hand_camera", "base", TABLE_HEIGHT)

        if demo_constants.is_real_robot():
            k = 3
            for i in xrange(k):
                for j in xrange(k):
                    q = tf.transformations.quaternion_from_euler(random.uniform(0, 2 * math.pi),
                                                                 random.uniform(0, 2 * math.pi),
                                                                 random.uniform(0, 2 * math.pi))

                    block = BlockState(id=str(len(self.gazebo_blocks)),
                                       pose=Pose(position=Point(x=0.45 + j * 0.15 + random.uniform(-1, 1) * 0.03,
                                                                y=-0.15 + i * 0.15 + random.uniform(-1, 1) * 0.03,
                                                                z=0.7725),
                                                 orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])))

                    self.gazebo_blocks.append(block)