Python broadcaster.TransformBroadcaster类代码示例

class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
#        self.param_one = rospy.get_param("~param_one", 100.0)
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.transform.rotation.w = 1.0
        self.pub.publish(msg)
        self.tfb.sendTransform((0,0,0), (0,0,0,1), rospy.Time.now(), '/pelican1/flyer_vicon', '/enu')

class RTbotController:
    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        # Subscribe to rtbot_motors
        rospy.Subscriber("rtbot_motors", Int16, self.rtbotMotorsCallback)

        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
    def poll(self):
        (x, y, theta) = self.arduino.rtbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        # Possible issue: If we were commanding the robot via Twist messages
        # then we would have some sensible values to put here.  But by setting
        # the motor commands directly, we don't have access to plausible values
        # for the forward and angular speeds.  Does this even matter???
        #odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.x = 0
        odom.twist.twist.linear.y = 0
        #odom.twist.twist.angular.z = self.angularSpeed
        odom.twist.twist.angular.z = 0

        self.odomPub.publish(odom)

    def stop(self):
        self.stopped = True
        self.arduino.rtbot_set_motors(0)
            
    def rtbotMotorsCallback(self, msg):
        self.arduino.rtbot_set_motors(msg.data)    

    def __init__(self, arduino):
        self.arduino = arduino
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param('~timeout', 1.0)
        self.stopped = False
                 
        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['Kp'] = rospy.get_param("~Kp", 4)
	pid_params['Ki'] = rospy.get_param("~Ki", 100)
        pid_params['Kd'] = rospy.get_param("~Kd", 1)
        
        
        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        
        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)
            
        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)
        
        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
                
        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0
                        
        now = rospy.Time.now()    
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # internal data        
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0                     # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0             # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now

        # subscriptions
        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self.cmdVelCallback)
        
        # Clear any old odometry info
        self.arduino.reset_encoders()
        
        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        
        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")

    def __init__(self, rate = 30.0, filename = None):
        # Marker server
        self.server = InteractiveMarkerServer('camera_marker')
        # TF broadcaster
        self.tf = TransformBroadcaster()

        # Marker pose
        self.pose_mutex = Lock()
        self.marker_position = (0.0, 0.0, 0.0)
        self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Load init position
        self.filename = filename
        if self.filename:
            with open(self.filename, 'r') as stream:
                init_data = yaml.load(stream)['init_position']
                self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
                self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Register shutdown callback
        rospy.on_shutdown(self.shutdown) 

        # Add marker
        self.add_6DOF()

        # Timer for TF broadcaster
        rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)

 def start(self):
     rospy.init_node('python_node_example')
     self.tfb = TransformBroadcaster()
     self.init_params()
     self.init_publishers()
     self.init_timers()
     rospy.spin()

 def __init__(self):
     self.ticks_meter = float(100)  # The number of wheel encoder ticks per meter of travel
     self.base_width = float(0.129) # The wheel base width in meters
     
     self.base_frame_id = 'base_link' # the name of the base frame of the robot
     self.odom_frame_id = 'odom' # the name of the odometry reference frame
     
     self.encoder_min = -32768
     self.encoder_max = 32768
     self.encoder_low_wrap = (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min 
     self.encoder_high_wrap = (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min 
     
     # internal data
     self.enc_left = None        # wheel encoder readings
     self.enc_right = None
     self.left = 0               # actual values coming back from robot
     self.right = 0
     self.x = 0                  # position in xy plane 
     self.y = 0
     self.th = 0
     self.dx = 0                 # speeds in x/rotation
     self.dr = 0
     self.then = 0
     
     self.odomPub = rospy.Publisher("/odom", Odometry,queue_size=10)
     self.odomBroadcaster = TransformBroadcaster()

   def __init__(self):
       print("init")
       port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
       self.create=Create(port)

       self.bump=False
       self.hz=30
       self.rate = 1. / self.hz
       self.speed = 200
       self.turn = 50
       self.distance = 85
       self.threshold = .5
       self.angle = 18
       self.lock = Lock()
       self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)

       self.odomPub=rospy.Publisher('odom', Odometry)
       self.odomBroadcaster=TransformBroadcaster()
       self.then=datetime.now()
       self.x=0
       self.y=0
       self.th=0
       self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
       
       self.create.update = self.sense

    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 282.5))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.26)) # The wheel base width in meters
        
        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 

        
        self.lowLevelInits()
        
        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        rospy.Subscriber("Nav_State", UInt32, self.navState)
        self.odomPub = rospy.Publisher("odom", Odometry)
        self.odomBroadcaster = TransformBroadcaster()

    def __init__(self, arduino):
        self.arduino = arduino
        self.stopped = False
        
        self.cmd = 1

        self.forwardSpeed = 0
        self.angularSpeed = 0
                 
        # Subscribe to cmd_vel
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        rospy.Subscriber("cmd_vel2", Twist, self.cmdVel2Callback)
        s = rospy.Service("pick_cmd_vel", PickCmdVel, self.pickCmdVel)

        # Subscribe to arbot_play_led and arbot_advance_led
        rospy.Subscriber("arbot_play_led", Bool, self.arbotPlayLedCallback)
        rospy.Subscriber("arbot_advance_led", Bool, self.arbotAdvanceLedCallback)
        
        # Setup the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        
        # Setup blob publisher
#        self.blob_publisher = rospy.Publisher('blobs', Blobs, queue_size=10)
        rospy.loginfo("Started ARbot controller.")

    def __init__(self):
        rospy.init_node('base_control')
        self.finished = Event()
        self.controller = Controller('/dev/ttyS0')

        rospy.loginfo("Started base_control")
        print "Started base_control"

        # odom...
        self.rate = float(rospy.get_param("~base_controller_rate", 20))
        now = rospy.Time.now()

        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = now + self.t_delta
        self.then = now # time for determining dx/dy

        # internal data
        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0                 # rotation in radians
        self.vx = 0.0
        self.vy = 0.0
        self.vth = 0.0

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.CMD_RATE =2 
        self.newTwistCmd = 0

        #self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        #self.telnet_ip = rospy.get_param('~telnet_ip', "192.168.1.107")
        self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
        #rospy.loginfo("Using port: %s" % self.port)
        rospy.loginfo("Using telnet: %s" % self.telnet_ip)

        #self.robot = Botvac(self.port)
        #self.robot = Huey(self.telnet_ip)
        self.robot = Botvac(self.telnet_ip)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
        self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()
        self.cmd_vel = [0, 0]
        self.old_vel = self.cmd_vel

    def __init__(self):
        #############################################################################
        rospy.init_node("process")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)

        #### parameters #######
        self.rate = rospy.get_param("~rate", 50.0)  # the rate at which to publish the transform

        self.base_frame_id = rospy.get_param("~base_frame_id", "base_link")  # the name of the base frame of the robot
        self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom")  # the name of the odometry reference frame
        self.laser_frame_id = rospy.get_param("~laser_frame_id", "laser_link")
        self.map_frame_id = rospy.get_param("~map_frame_id", "map")
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta

        # internal data

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        self.th_pre = 0
        self.dx = 0  # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()

        self.qw = 0
        self.qx = 0
        self.qy = 0
        self.qz = 0
        self.laser = LaserScan()
        # subscriptions

        rospy.Subscriber("qx", Float32, self.qx_update)
        rospy.Subscriber("qy", Float32, self.qy_update)
        rospy.Subscriber("qz", Float32, self.qz_update)
        rospy.Subscriber("qw", Float32, self.qw_update)
        rospy.Subscriber("th", Float32, self.th_update)
        rospy.Subscriber("scan", LaserScan, self.laser_update)
        rospy.Subscriber("px", Float32, self.x_update)
        rospy.Subscriber("py", Float32, self.y_update)
        rospy.Subscriber("dx", Float32, self.dx_update)
        # rospy.Subscriber('dr',Float32,self.dr_update)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
        self.laserPub = rospy.Publisher("laser_scan", LaserScan, queue_size=20)
        self.odomBroadcaster = TransformBroadcaster()
        self.laserBroadcaster = TransformBroadcaster()

class DummyViconNode(object):
    """
    Example python node including reading parameters, publishers and subscribers, and timer.
    """
    def __init__(self):
        # Remember that init_node hasn't been called yet so only do stuff here that
        # doesn't require node handles etc.
        pass

    def start(self):
        rospy.init_node('python_node_example')
        self.tfb = TransformBroadcaster()
        self.init_params()
        self.init_publishers()
        self.init_timers()
        rospy.spin()
        
    def init_params(self):
        pass
        self.tf_ref_frame_id = rospy.get_param("~tf_ref_frame_id", "/enu")
        self.tf_tracked_frame_id = rospy.get_param("~tf_tracked_frame_id", "/pelican1/flyer_vicon")
        self.dummy_position = rospy.get_param("~dummy_position", (0.0, -0.3593, -0.05))
        self.dummy_orientation = rospy.get_param("~dummy_orientation", (0.5, 0.5, -0.5, 0.5)) # xyzw
        self.enable_tf_broadcast = rospy.get_param("~enable_tf_broadcast", False)
        
#        self.param_two = rospy.get_param("~param_two", False)
    
    def init_publishers(self):
        self.pub = rospy.Publisher('vicon_recv_direct/output', TransformStamped)
        
    def init_timers(self):
        self.vicon_timer = Timer(rospy.Duration(1/120.0), self.vicon_timer_callback) # Call at 10 Hz
                                          
    def vicon_timer_callback(self, event):
        msg = TransformStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.tf_ref_frame_id
        msg.child_frame_id = self.tf_tracked_frame_id
        msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z = self.dummy_position
        (msg.transform.rotation.x, msg.transform.rotation.y, 
         msg.transform.rotation.z, msg.transform.rotation.w) = self.dummy_orientation
        self.pub.publish(msg)
        if self.enable_tf_broadcast:
            self.tfb.sendTransform(self.dummy_position, self.dummy_orientation, 
                                   rospy.Time.now(), self.tf_tracked_frame_id, self.tf_ref_frame_id)

	def __init__(self):
		port = rospy.get_param('/brown/irobot_create/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher('/sensorPacket', SensorPacket, queue_size=10)
		self.odomPub = rospy.Publisher('/odom',Odometry, queue_size=10)
		self.odomBroadcaster = TransformBroadcaster()
		self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
		self.then = datetime.now() 
		self.x = 0
		self.y = 0
		self.th = 0
		self.create.update = self.sense

    def __init__(self):
    #############################################################################
        rospy.init_node("diff_tf")
        self.nodename = rospy.get_name()
        rospy.loginfo("-I- %s started" % self.nodename)
        
        #### parameters #######
        self.rate = rospy.get_param('~rate',10.0)  # the rate at which to publish the transform
        self.ticks_meter = float(rospy.get_param('ticks_meter', 50))  # The number of wheel encoder ticks per meter of travel
        self.base_width = float(rospy.get_param('~base_width', 0.245)) # The wheel base width in meters
 
# emg - base_frame_id should be /robot 
#        self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot
        self.base_frame_id = rospy.get_param('~base_frame_id','robot')
        self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame
        
        self.encoder_min = rospy.get_param('encoder_min', -32768)
        self.encoder_max = rospy.get_param('encoder_max', 32768)
        self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
        self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
 
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        
        # internal data
        self.enc_left = None        # wheel encoder readings
        self.enc_right = None
        self.left = 0               # actual values coming back from robot
        self.right = 0
        self.lmult = 0
        self.rmult = 0
        self.prev_lencoder = 0
        self.prev_rencoder = 0
        self.x = 0                  # position in xy plane 
        self.y = 0
        self.th = 0
        self.dx = 0                 # speeds in x/rotation
        self.dr = 0
        self.then = rospy.Time.now()

# emg - adding in a holder for the quaternion
        self.orientation = Quaternion()

        # subscriptions
        rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
        rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
        rospy.Subscriber("Orientation_data", Quaternion, self.orientationCallback)
        self.odomPub = rospy.Publisher("odom", Odometry)
# emg - why the tf_broadcaster? is this the /odom -> /robot? if that's
#       the case, i thought we'd all agreed that these should be
#       explicitly tied together with a static_tf of "0 0 0 0 0 0"
        self.odomBroadcaster = TransformBroadcaster()