python类TransformBroadcaster()的实例源码

def __init__(self):
        # first let's load all parameters:
        self.frame_id = rospy.get_param("~frame_id", "odom_meas")
        self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame")
        self.base_frame_id = rospy.get_param("~base_frame_id", "base_meas")
        self.x0 = rospy.get_param("~x0", 0.0)
        self.y0 = rospy.get_param("~y0", 0.0)
        self.th0 = rospy.get_param("~th0", 0.0)
        self.pubstate = rospy.get_param("~pubstate", True)
        self.marker_id = rospy.get_param("~marker_id", 12)

        # setup other required vars:
        self.odom_offset = odom_conversions.numpy_to_odom(np.array([self.x0, self.y0, self.th0]),
                                                          self.frame_id)
        self.path_list = deque([], maxlen=PATH_LEN)

        # now let's create publishers, listeners, and subscribers
        self.br = tf.TransformBroadcaster()
        self.listener = tf.TransformListener()
        self.meas_pub = rospy.Publisher("meas_pose", Odometry, queue_size=5)
        self.path_pub = rospy.Publisher("meas_path", Path, queue_size=1)
        self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb)
        self.publish_serv = rospy.Service("publish_bool", SetBool, self.pub_bool_srv_cb)
        self.offset_serv = rospy.Service("set_offset", SetOdomOffset, self.offset_srv_cb)
        return

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)

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)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/pick_frame_name", String,
                                          self.set_pick_frame,
                                          queue_size=1)

        self.obj_pose_sub = rospy.Subscriber("/place_frame_name", String,
                                          self.set_place_frame,
                                          queue_size=1)

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                  self.broadcast_frames,
                                  queue_size=1)

        self.place_frame = ''
        self.pick_frame = ''
        self.tower_size = 1
        self.tower_size_sub = rospy.Subscriber("/tower_size",Int64,self.set_tower_size)

def __init__(self):
        self.marker_id = rospy.get_param("~marker_id", 12)
        self.frame_id = rospy.get_param("~frame_id", "odom_meas")
        self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame")
        self.count = rospy.get_param("~count", 50)

        # local vars:
        self.calibrate_flag = False
        self.calibrated = False
        self.calibrate_count = 0
        self.kb = kbhit.KBHit()
        self.trans_arr = np.zeros((self.count, 3))
        self.quat_arr = np.zeros((self.count, 4))
        self.trans_ave = np.zeros(3)
        self.quat_ave = np.zeros(3)

        # now create subscribers, timers, and publishers
        self.br = tf.TransformBroadcaster()
        self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb)
        rospy.on_shutdown(self.kb.set_normal_term)
        self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb)
        return

def _init_pubsub(self):
        self.joint_states_pub = rospy.Publisher('joint_states', JointState)
        self.odom_pub = rospy.Publisher('odom', Odometry)

        self.sensor_state_pub = rospy.Publisher('~sensor_state', TurtlebotSensorState)
        self.operating_mode_srv = rospy.Service('~set_operation_mode', SetTurtlebotMode, self.set_operation_mode)
        self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs)

        if self.drive_mode == 'twist':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel)
            self.drive_cmd = self.robot.direct_drive
        elif self.drive_mode == 'drive':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel)
            self.drive_cmd = self.robot.drive
        elif self.drive_mode == 'turtle':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Turtle, self.cmd_vel)
            self.drive_cmd = self.robot.direct_drive
        else:
            rospy.logerr("unknown drive mode :%s"%(self.drive_mode))

        self.transform_broadcaster = None
        if self.publish_tf:
            self.transform_broadcaster = tf.TransformBroadcaster()

def handle_image_msg(msg):
    assert msg._type == 'sensor_msgs/Image'

    with g_fusion_lock:
        g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec()))

        if g_fusion.last_state_mean is not None:
            pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)

            yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3])
            br = ros_tf.TransformBroadcaster()
            br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(), 'obj_fuse_centroid', 'velodyne')

            if last_known_box_size is not None:
                # bounding box
                marker = Marker()
                marker.header.frame_id = "obj_fuse_centroid"
                marker.header.stamp = rospy.Time.now()
                marker.type = Marker.CUBE
                marker.action = Marker.ADD
                marker.scale.x = last_known_box_size[2]
                marker.scale.y = last_known_box_size[1]
                marker.scale.z = last_known_box_size[0]
                marker.color = ColorRGBA(r=0., g=1., b=0., a=0.5)
                marker.lifetime = rospy.Duration()
                pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10)
                pub.publish(marker)

def __init__(self, name):
        self.robotname = rospy.get_param("~robot")
        self.br = tf.TransformBroadcaster()
        self.sub = rospy.Subscriber('/%s/base_pose_ground_truth' % self.robotname,
                                    Odometry,
                                    self.handle_robot_pose, 
                                    self.robotname)

def broadcast_position(pose, to_frame, from_frame):
    broadcaster = tf.TransformBroadcaster()
    pose = pose
    broadcaster.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
        rospy.Time.now(),
        to_frame,
        from_frame
    )

def publish_transform(name, pos):
    broadcast = tf.TransformBroadcaster()
    while(True):
        broadcast.sendTransform(tuple(pos), [0,0,0,1],
                                    rospy.Time.now(),
                                    name,
                                    "root")
        rospy.sleep(1)

def __init__(self, topic='/sensor_values'):
        self.bx = SmartBaxter('left')

        self.br = tf.TransformBroadcaster()
        self.tl = tf.TransformListener()

def __init__(self, topic='/sensor_values'):
        self.bx = SmartBaxter('left')

        self.br = tf.TransformBroadcaster()
        self.tl = tf.TransformListener()

def __init__(self):
        self.tfBuffer = tf2_ros.Buffer()
        self.listen = tf2_ros.TransformListener(self.tfBuffer)

        self.listener = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

def __init__(self):
        self.tfBuffer = tf2_ros.Buffer()
        self.listen = tf2_ros.TransformListener(self.tfBuffer)

        self.listener = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

def __init__(self):
        self.tfBuffer = tf2_ros.Buffer()
        self.listen = tf2_ros.TransformListener(self.tfBuffer)

        self.listener = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        self.listen = tf.TransformListener()
        self.broadcast = tf.TransformBroadcaster()

        self.obj_pose_sub = rospy.Subscriber("/num_objects", Int64,
                                          self.broadcast_frame,
                                          queue_size=1)

def __init__(self):
        super(PerceptionNode, self).__init__('p_node')

        self.transition_table = {
            # If calibration has already happened, allow skipping it
            'initial': {'q': self._perceive},
            'perceive': {'q': self._post_perceive},
            }

        # Hardcoded place for now
        self.tl = tf.TransformListener()
        self.num_objects = 0
        # Would this work too? Both tf and tf2 have (c) 2008...
        # self.tf2 = tf2_ros.TransformListener()
        self.n_objects_sub = rospy.Subscriber("/num_objects", Int64,
                                              self.update_num_objects,
                                              queue_size=1)
        self.perception_pub = rospy.Publisher("/perception/enabled",
                                              Bool,
                                              queue_size=1)

        self.br = tf.TransformBroadcaster()

        # Dict to map imarker names and their updated poses
        self.int_markers = {}

        # Ideally this call would be in a Factory/Metaclass/Parent
        self.show_options()

def __init__(self, frequency):

        self.tag_names = rospy.get_param("tag_names")
        self.frame_id = rospy.get_param("~frame_id")
        self.transforms = rospy.get_param("tags")
        self.rate = rospy.Rate(frequency)
        #self.sub = rospy.Subscriber(self.tag_position_topic, PoseArray, self.tag_position_cb)

        self.br = tf.TransformBroadcaster()

        self.make_transforms()

def __init__(self):
        """Set up class variables, initialize broadcaster and start subscribers."""

        # Create broadcasters
        self.br_head = tf.TransformBroadcaster()
        self.br_arm = tf.TransformBroadcaster()

        # Create subscribers
        rospy.Subscriber("bax_head/pose", PoseStamped, self.head_callback, queue_size=1)
        rospy.Subscriber("bax_arm/pose", PoseStamped, self.arm_callback, queue_size=1)

        # Main while loop.
        rate = rospy.Rate(50)
        while not rospy.is_shutdown():
            rate.sleep()

def __init__(self):
        """Set up class variables, initialize broadcaster and start subscribers."""

        # Create broadcasters
        self.br_head = tf.TransformBroadcaster()
        self.br_rods = tf.TransformBroadcaster()

        # Create subscribers
        rospy.Subscriber("bax_head/pose", PoseStamped, self.head_callback, queue_size=1)
        rospy.Subscriber("rods/pose", PoseStamped, self.rod_callback, queue_size=1)

        # Main while loop.
        rate = rospy.Rate(50)
        while not rospy.is_shutdown():
            rate.sleep()

def __init__(self):
        self.rate = rospy.get_param('~rate', 10.0)  # the rate at which to publish the transform
        self.submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of', '').split(';')

        self.tfBroadcaster = tf.TransformBroadcaster()
        self.link_states_msg = None

        self.model_states_msg = None
        self.model_cache = {}
        self.updatePeriod = 1. / self.rate
        self.enable_publisher_marker = False
        self.enable_publisher_tf = False

        self.markerPub = rospy.Publisher('/model_marker', Marker, queue_size=10)
        self.modelStatesSub = rospy.Subscriber('gazebo/model_states', ModelStates, self.on_model_states_msg, queue_size=1)
        self.linkStatesSub = rospy.Subscriber('gazebo/link_states', LinkStates, self.on_link_states_msg, queue_size=1)
        rate_sleep = rospy.Rate(self.rate)

        # Main while loop
        while not rospy.is_shutdown():
            if self.enable_publisher_marker and self.enable_publisher_tf:
                self.publish_tf()
                self.publish_marker()

            rate_sleep.sleep()

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.