Python tf.TransformBroadcaster() Examples

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() 

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, 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() 

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') 

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") 

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) 

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() 

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) 

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) 

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. 

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) 

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() 

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) 

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) 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		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')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

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

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

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

#		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)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

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) 

def handle_turtle_pose(msg):
    br = tf.TransformBroadcaster()
    LaserScan laser_tf 

def __init__(self,options=None):     
        self.options = options
        self.warntime = time.time()
        self.USB = False
           
        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, queue_size=2)
        self.pub_PID = rospy.Publisher("/pid", PidMSG,queue_size=50)

        # PIDs for R,P,Y,THROTTLE
        self.PID = (PID(), PID(), PID(), PID()) # XY, Throttle, Yaw
        self.PIDActive = (True, True, True, True) # XY, YAw, Throttle on/off
        self.PIDDelay = 0.0
        self.PIDSource = Source.Qualisys
        self.PIDControl = True
        self.goal = [0.0, 0.0, 0.0, 0.0] # X Y Z YAW
        self.prev_msg = None
        self.PIDSetCurrentAuto = True

        self.thrust = (40., 90., 66.) # Min, Max, Base thrust
        self.distFunc = fLIN
        self.wandControl = False

        # Dynserver
        self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure) 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		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')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

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

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

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

#		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)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

def __init__(self, sub_tf, pub_tf, parent=None):
        super(TrackManager, self).__init__(parent)



        # Initialise gui
        self.ui = Ui_ScrollAreaTracker()
        self.ui.setupUi(self)

        # Ros stuff
        self.sub_tf = sub_tf #tf.TransformListener()
        self.pub_tf = pub_tf #tf.TransformBroadcaster()

        # Initialise Trackers
        self.trackers = [KinectTracker(self), QualisysTracker(self)]
        for t in self.trackers:
            t.sig_started.connect(self.started)
            t.sig_error.connect(self.startFailed)


        # KINECT STUFF
        self.trackers[0].sig_inview.connect(self.sig_inView.emit)
        #self.ui.checkBox_autoCapture

        self.trackers[0].sig_backgroundStep.connect(self.updatePB)
        self.ui.comboBox_kinMethod.currentIndexChanged.connect(self.trackers[0].setMethod)
        self.ui.doubleSpinBox_kinMaxDepth.valueChanged.connect(self.trackers[0].setMaxDepth)
        self.ui.doubleSpinBox_kinFG.valueChanged.connect(self.trackers[0].setForegroundDist)
        self.ui.pushButton_reset.clicked.connect(lambda : self.trackers[0].resetBackground(self.ui.doubleSpinBox_kinObsTime.value()))


        self.ui.doubleSpinBox_kinDetSize.valueChanged.connect(self.trackers[0].setSize)
        self.ui.doubleSpinBox_tol.valueChanged.connect(self.trackers[0].setSizeTolerance)
        self.ui.spinBox_kernelSize.valueChanged.connect(self.trackers[0].setKernel)
        self.ui.spinBox_kernelIter.valueChanged.connect(self.trackers[0].setIterations)
        self.ui.comboBox_prio.currentIndexChanged.connect(self.trackers[0].setPriority)
        self.ui.comboBox_kinDepth.currentIndexChanged.connect(self.trackers[0].setDepthEstMode)

        self.tracker = None
        self.tracking = False

        # Connections GUI to GUI
        self.ui.comboBox_tracker.clear()
        self.ui.comboBox_tracker.addItems([t.name for t in self.trackers])
        self.ui.comboBox_tracker.currentIndexChanged.connect(self.switchTracker)
        self.ui.comboBox_tracker.currentIndexChanged.emit(self.ui.comboBox_tracker.currentIndex()) # Force update

        self.readSettings()

        # Connections from GUI
        self.q01 = (self.ui.comboBox_q0.currentIndex(), self.ui.comboBox_q1.currentIndex())
        self.ui.pushButton_trackStart.clicked.connect(self.startStopClicked)
        self.ui.comboBox_q0.currentIndexChanged.connect(lambda x: self.qualisysFrameChanged(0,x))
        self.ui.comboBox_q1.currentIndexChanged.connect(lambda x: self.qualisysFrameChanged(1,x)) 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		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')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

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

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

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

#		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)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		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')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

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

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

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

#		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)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		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')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

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

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

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

#		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)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

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)

        self.blue1 = (0.641782207489,
                  -0.224464386702,
                  -0.363829042912)
        self.red1 = (0.69,
                  -0.31,
                  -0.363829042912)
        self.green1 = (0.68,
                  -0.10,
                  -0.363829042912)
        self.yellow1 = (0.68,
                  -0.20,
                  -0.363829042912)

        table_pos = (0.5, 0., 0.863-0.5)
        self.table_pos, self.table_rot = pm.toTf(kdl.Frame(
            kdl.Rotation.RotZ(-np.pi/2.),
            kdl.Vector(*table_pos)))
        box_pos = (0.82, -0.4, 0.863-0.5+0.1025)
        self.box_pos, self.box_rot = pm.toTf(kdl.Frame(
            kdl.Rotation.RotZ(1.5),
            kdl.Vector(*box_pos)))
        box_pos = (0.82, -0.4, 0.863-0.5+0.1025)
        self.box_pos, self.box_rot = pm.toTf(kdl.Frame(
            kdl.Rotation.RotZ(1.5),
            kdl.Vector(*box_pos)))
        b1_pos = (0.78, -0.03, 0.863-0.5+0.0435)
        self.block1_pos, self.block1_rot = pm.toTf(kdl.Frame(
            kdl.Rotation.RotZ(-np.pi/2.),
            kdl.Vector(*b1_pos)))
        b1_pos = (0.73, 0.12, 0.863-0.5+0.0435)
        self.block2_pos, self.block2_rot = pm.toTf(kdl.Frame(
            kdl.Rotation.RotZ(-np.pi/2.+0.07),
            kdl.Vector(*b1_pos))) 

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) 

def _update_lsm_odom(self,msg):
        self._OdomData2.header.stamp = msg.header.stamp
        self._OdomData2.header.seq += 1
        
        (r, p, w) = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
        self._MsgData.odom_yaw_angle_rad = ( w + math.pi) % (2 * math.pi ) - math.pi
        
        rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
        pos = (msg.pose.pose.position.x,msg.pose.pose.position.y,0.0)
        
        if (False == self.has_recved_lsm):
            self._prev_lsm_update_time = msg.header.stamp.to_sec()
            self._prev_lsm_x = msg.pose.pose.orientation.x
            self._prev_lsm_y = msg.pose.pose.orientation.y
            self._prev_lsm_w = w
            self.has_recved_lsm = True
            dxdt=0.0
            dydt=0.0
            dwdt=0.0 
        else:
            dt = (msg.header.stamp.to_sec() - self._prev_lsm_update_time)
            dxdt= (msg.pose.pose.position.x - self._prev_lsm_x)/dt
            dydt= (msg.pose.pose.position.y - self._prev_lsm_y)/dt
            dwdt= (w - self._prev_lsm_w)/dt
            self._prev_lsm_x = msg.pose.pose.position.x
            self._prev_lsm_y = msg.pose.pose.position.y
            self._prev_lsm_w = w
            self._prev_lsm_update_time = msg.header.stamp.to_sec()
            
            dxdt = (dxdt+self._OdomData1.twist.twist.linear.x)/2.0
            dydt = (dydt+self._OdomData1.twist.twist.linear.y)/2.0
            dwdt = (dwdt+self._OdomData1.twist.twist.angular.z)/2.0
        
        self._OdomData2.twist.twist.linear.x = self._OdomData1.twist.twist.linear.x #dxdt
        self._OdomData2.twist.twist.linear.y = self._OdomData1.twist.twist.linear.y #dydt
        self._OdomData2.twist.twist.linear.z = 0.0
        self._OdomData2.twist.twist.angular.x = 0.0
        self._OdomData2.twist.twist.angular.y = 0.0
        self._OdomData2.twist.twist.angular.z = self._OdomData1.twist.twist.angular.z #dwdt
        self._OdomData2.pose = msg.pose
        
        
        self._OdomPub2.publish(self._OdomData2)        
        br = tf.TransformBroadcaster()
        br.sendTransform(pos,
                         rot,
                         msg.header.stamp,
                         "base_link",
                         "odom")