Example #1
0
class NeatoNode:
	
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.CMD_RATE =2 

        self.port = rospy.get_param('~port', "/dev/ttyACM0")
        rospy.loginfo("Using port: %s" % self.port)

        self.robot = Botvac(self.port)

        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 spin(self):
        encoders = [0, 0]

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))

        scan.angle_min =0.0 
        scan.angle_max =359.0*pi/180.0 
        scan.angle_increment =pi/180.0 
        scan.range_min = 0.020
        scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')

        button = Button()
        sensor = Sensor()
        self.robot.setBacklight(1)
        self.robot.setLED("Green")
        # main loop of driver
        r = rospy.Rate(5)
        cmd_rate= self.CMD_RATE
        self.robot.requestScan()

        while not rospy.is_shutdown():
            # notify if low batt
            #if self.robot.getCharger() < 10:
            #    print "battery low " + str(self.robot.getCharger()) + "%"
            # get motor encoder values
            left, right = self.robot.getMotors()

            cmd_rate = cmd_rate-1
            if cmd_rate ==0:
		    # send updated movement commands
		    #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
                    # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
		    self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    cmd_rate = self.CMD_RATE

            self.old_vel = self.cmd_vel

            # prepare laser scan
            scan.header.stamp = rospy.Time.now()
           
            #scan.ranges, scan.intensities = self.robot.getScanRanges()
            scan.ranges = self.robot.getScanRanges()
            self.robot.requestScan()

            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left =  (left - encoders[0])/1000.0
            d_right =  (right - encoders[1])/1000.0
            encoders = [left, right]

	    #print d_left, d_right, encoders

            dx = (d_left+d_right)/2
            dth = (d_right-d_left)/(self.robot.base_width/1000.0)

            x = cos(dth)*dx
            y = -sin(dth)*dx
            self.x += cos(self.th)*x - sin(self.th)*y
            self.y += sin(self.th)*x + cos(self.th)*y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th/2.0)
            quaternion.w = cos(self.th/2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx/dt
            odom.twist.twist.angular.z = dth/dt


            # sensors
            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()

            # buttons
            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()


            # publish everything
            self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
                                                                     quaternion.w), then, "base_footprint", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)
            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
                if b == 1:
                    button.value = b
                    button.name = button_enum[idx]
                    self.buttonPub.publish(button)

            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
                if b == 1:
                    sensor.value = b
                    sensor.name = sensor_enum[idx]
                    self.sensorPub.publish(sensor)
            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setBacklight(0)
        self.robot.setLED("Off")
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def sign(self,a):
        if a>=0:
		return 1
	else:
		return-1

    def cmdVelCb(self,req):
        x = req.linear.x * 1000
        th = req.angular.z * (self.robot.base_width/2)
        k = max(abs(x-th),abs(x+th))
        # sending commands higher than max speed will fail

        if k > self.robot.max_speed:
            x = x*self.robot.max_speed/k; th = th*self.robot.max_speed/k

        self.cmd_vel = [int(x-th), int(x+th)]
Example #2
0
class NeatoNode:
	
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.CMD_RATE =2 

        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        rospy.loginfo("Using port: %s" % self.port)

        self.robot = Botvac(self.port)

        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 spin(self):
        encoders = [0, 0]

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))

        scan.angle_min =0.0 
        scan.angle_max =359.0*pi/180.0 
        scan.angle_increment =pi/180.0 
        scan.range_min = 0.020
        scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')

        button = Button()
        sensor = Sensor()
        self.robot.setBacklight(1)
        self.robot.setLED("Green")
        # main loop of driver
        r = rospy.Rate(20)
        cmd_rate= self.CMD_RATE

        while not rospy.is_shutdown():
            # notify if low batt
            #if self.robot.getCharger() < 10:
            #    print "battery low " + str(self.robot.getCharger()) + "%"
            # get motor encoder values
            left, right = self.robot.getMotors()

            cmd_rate = cmd_rate-1
            if cmd_rate ==0:
		    # send updated movement commands
		    #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
                    # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
		    self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
		    cmd_rate = self.CMD_RATE

            self.old_vel = self.cmd_vel

            # prepare laser scan
            scan.header.stamp = rospy.Time.now()
           
            self.robot.requestScan()
            scan.ranges = self.robot.getScanRanges()

            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left =  (left - encoders[0])/1000.0
            d_right =  (right - encoders[1])/1000.0
            encoders = [left, right]

	    #print d_left, d_right, encoders

            dx = (d_left+d_right)/2
            dth = (d_right-d_left)/(self.robot.base_width/1000.0)

            x = cos(dth)*dx
            y = -sin(dth)*dx
            self.x += cos(self.th)*x - sin(self.th)*y
            self.y += sin(self.th)*x + cos(self.th)*y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th/2.0)
            quaternion.w = cos(self.th/2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx/dt
            odom.twist.twist.angular.z = dth/dt


            # sensors
            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()

            # buttons
            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()


            # publish everything
            self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
                                                                     quaternion.w), then, "base_footprint", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)
            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
                if b == 1:
                    button.value = b
                    button.name = button_enum[idx]
                    self.buttonPub.publish(button)

            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
                if b == 1:
                    sensor.value = b
                    sensor.name = sensor_enum[idx]
                    self.sensorPub.publish(sensor)
            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setBacklight(0)
        self.robot.setLED("Off")
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def sign(self,a):
        if a>=0:
		return 1
	else:
		return-1

    def cmdVelCb(self,req):
        x = req.linear.x * 1000
        th = req.angular.z * (self.robot.base_width/2)
        k = max(abs(x-th),abs(x+th))
        # sending commands higher than max speed will fail

        if k > self.robot.max_speed:
            x = x*self.robot.max_speed/k; th = th*self.robot.max_speed/k

        self.cmd_vel = [int(x-th), int(x+th)]
Example #3
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        self.lds = rospy.get_param('~lds', True)
        rospy.loginfo("Using port: %s" % self.port)

        self.robot = Botvac(self.port, self.lds)

        rospy.Subscriber("cmd_dist", Movement, self.cmdMovementCb)
        self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=1)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1)
        self.buttonPub = rospy.Publisher('button', Button, queue_size=1)
        self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=1)
        self.accelerationPub = rospy.Publisher('acceleration',
                                               Vector3Stamped,
                                               queue_size=1)
        self.wallPub = rospy.Publisher('wall', Range, queue_size=1)
        self.drop_leftPub = rospy.Publisher('drop_left', Range, queue_size=1)
        self.drop_rightPub = rospy.Publisher('drop_right', Range, queue_size=1)
        self.magneticPub = rospy.Publisher('magnetic', Sensor, queue_size=1)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.Service('set_info_led', SetLed, self.setInfoLed)
        rospy.Service('play_sound', PlaySound, self.playSound)
        rospy.Service('set_lds', SetBool, self.setLDS)

        self.cmd_vel = 0
        self.cmd_dist = [0, 0]
        self.old_dist = self.cmd_dist
        self.encoders = [0, 0]

    def spin(self):
        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))
        scan.angle_min = -3.13
        scan.angle_max = +3.13
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id="odom"),
                        child_frame_id='base_link')
        self.odomPub.publish(odom)

        button = Button()
        sensor = Sensor()
        magnetic = Sensor()
        range_sensor = Range()
        range_sensor.radiation_type = 1
        #range_sensor.field_of_view =
        range_sensor.min_range = 0.0
        range_sensor.max_range = 0.255
        acceleration = Vector3Stamped()
        self.robot.setBacklight(1)
        self.robot.setLED("info", "blue", "solid")
        # main loop of driver
        r = rospy.Rate(5)
        loop_counter = 0
        try:
            while not rospy.is_shutdown():

                if loop_counter == 4:
                    self.set_battery_status()
                    self.publish_scan(scan)
                    loop_counter = 0
                else:
                    loop_counter += 1

                self.publish_odom(odom)
                self.publish_buttons(button)
                drop_left, drop_right, ml, mr = self.publish_analog(
                    acceleration, range_sensor, magnetic)
                lw, rw, lsb, rsb, lfb, rfb = self.publish_digital(sensor)

                # send updated movement commands
                if self.violate_safety_constraints(drop_left, drop_right, ml,
                                                   mr, lw, rw, lsb, rsb, lfb,
                                                   rfb):
                    self.robot.setMotors(0, 0, 0)
                    self.cmd_vel = [0, 0]
                elif self.cmd_vel != self.old_vel:
                    self.robot.setMotors(self.cmd_dist[0], self.cmd_dist[1],
                                         self.cmd_vel)
                    # reset command distance and speed
                    self.robot.cmd_dist = [0, 0]
                    self.robot.cmd_vel = 0
            # wait, then do it again
                r.sleep()

            # shut down
            self.robot.setMotors(0, 0, 0)
            self.robot.setBacklight(0)
            self.robot.setLED("Battery", "Green", "Off")
            self.robot.setLED("Info", "Blue", "Off")
            self.robot.setLDS("off")
            self.robot.setTestMode("off")
        except:
            exc_info = sys.exc_info()
            traceback.print_exception(*exc_info)
            self.robot.setMotors(0, 0, 0)
            self.robot.setBacklight(0)
            self.robot.setLED("Battery", "Green", "Off")
            self.robot.setLED("Info", "Red", "Solid")
            self.robot.setLDS("off")
            self.robot.setTestMode("off")

    def cmdVelCb(self, req):
        x = req.linear.x * 1000
        th = req.angular.z * (self.robot.base_width / 2)
        k = max(abs(x - th), abs(x + th))
        # sending commands higher than max speed will fail
        if k > self.robot.max_speed:
            x = x * self.robot.max_speed / k
            th = th * self.robot.max_speed / k
        self.cmd_vel = [int(x - th), int(x + th)]

    def cmdMovementCb(self, req):
        k = req.vel
        # sending commands higher than max speed will fail
        if k > self.robot.max_speed:
            k = self.robot.max_speed
            ros.logwarn(
                "You have set the speed to more than the maximum speed of the neato. For safety reasons it is set to %d",
                self.robot.max_speed)
        self.robot.cmd_vel = k
        self.robot.cmd_dist = [req.x_dist, req.y_dist]

    def publish_odom(self, odom):
        # get motor encoder values
        left, right = self.robot.getMotors()

        d_left = (left - self.encoders[0]) / 1000.0
        d_right = (right - self.encoders[1]) / 1000.0
        self.encoders = [left, right]

        dx = (d_left + d_right) / 2
        dth = (d_right - d_left) / (self.robot.base_width / 1000.0)

        x = cos(dth) * dx
        y = -sin(dth) * dx
        self.x += cos(self.th) * x - sin(self.th) * y
        self.y += sin(self.th) * x + cos(self.th) * y
        self.th += dth

        # prepare tf from base_link to odom
        quaternion = Quaternion()
        quaternion.z = sin(self.th / 2.0)
        quaternion.w = cos(self.th / 2.0)

        dt = (odom.header.stamp - rospy.Time.now()).secs

        # prepare odometry
        odom.header.stamp = rospy.Time.now()
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        odom.twist.twist.linear.x = dx / dt
        odom.twist.twist.angular.z = dth / dt

        # publish everything
        self.odomPub.publish(odom)
        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(), "base_link", "odom")

    def publish_scan(self, scan):
        scan.header.stamp = rospy.Time.now()
        scan.ranges = self.robot.getScanRanges()
        self.scanPub.publish(scan)

    def publish_analog(self, acceleration, range_sensor, magnetic):
        # analog sensors
        ax, ay, az, ml, mr, wall, drop_left, drop_right = self.robot.getAnalogSensors(
        )
        acceleration.header.stamp = rospy.Time.now()
        # convert mG to m/s^2
        acceleration.vector.x = ax * 9.80665 / 1000.0
        acceleration.vector.y = ay * 9.80665 / 1000.0
        acceleration.vector.z = az * 9.80665 / 1000.0
        range_sensor.header.stamp = rospy.Time.now()

        self.accelerationPub.publish(acceleration)

        range_sensor.range = wall / 1000.0
        self.wallPub.publish(range_sensor)
        range_sensor.range = drop_left / 1000.0
        self.drop_leftPub.publish(range_sensor)
        range_sensor.range = drop_right / 1000.0
        self.drop_rightPub.publish(range_sensor)

        magnetic_enum = ("Left_Sensor", "Right_Sensor")
        for idx, val in enumerate((ml, mr)):
            magnetic.value = val
            magnetic.name = magnetic_enum[idx]
            self.magneticPub.publish(magnetic)
        return drop_left, drop_right, ml, mr

    def publish_buttons(self, button):
        btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons(
        )

        button_enum = ("Soft_Button", "Up_Button", "Start_Button",
                       "Back_Button", "Down_Button")
        for idx, b in enumerate(
            (btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
            if b == 1:
                button.value = b
                button.name = button_enum[idx]
                self.buttonPub.publish(button)

    def publish_digital(self, sensor):
        lsb, rsb, lfb, rfb, lw, rw = self.robot.getDigitalSensors()

        sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper",
                       "Right_Bumper", "Left_Wheel", "Right_Wheel")
        for idx, b in enumerate((lsb, rsb, lfb, rfb, lw, rw)):
            if b == 1:
                sensor.value = b
                sensor.name = sensor_enum[idx]
                self.sensorPub.publish(sensor)
        return lw, rw, lsb, rsb, lfb, rfb

    def set_battery_status(self):
        # notify if low batt
        charge = self.robot.getCharger()
        if charge < 10:
            #print "battery low " + str(self.robot.getCharger()) + "%"
            self.robot.setLED("battery", "red", "pulse")
        elif charge < 25:
            self.robot.setLED("battery", "yellow", "solid")
        else:
            self.robot.setLED("battery", "green", "solid")
        rospy.loginfo_throttle(60,
                               "Current battery status: " + str(charge) + "%")

    def violate_safety_constraints(left_drop, right_drop, *digital_sensors):
        if left_drop > 30 or right_drop > 30:
            print "safety constraint violated by drop sensor"
            return True
        else:
            for sensor in digital_sensors:
                if sensor == 1:
                    print "safety constraint violated by digital sensor"
                    return True
        return False

    def setInfoLed(self, data):
        self.robot.setLED("info", data.color, data.status)
        return SetLedResponse()

    def playSound(self, data):
        self.robot.playSound(data.soundid)
        return PlaySoundResponse()

    def setLDS(self, data):
        if data.data:
            self.robot.setLDS("on")
        else:
            self.robot.setLDS("off")
        return SetBoolResponse(True, "")
Example #4
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

        self.CMD_RATE = 2

        self.port = rospy.get_param('~port', "/dev/ttyUSB0")
        rospy.loginfo("Using port: %s" % self.port)

        self.odom_topic = rospy.get_param('~odom_topic', "odom")
        self.dist_topic = rospy.get_param('~dist_topic', "dist")
        self.cmd_vel_topic = rospy.get_param('~cmd_vel_topic', "cmd_vel")
        self.base_frame = rospy.get_param('~base_frame', "base_link")
        self.odom_frame = rospy.get_param('~odom_frame', "odom")

        self.cmd_angle_instead_rotvel = rospy.get_param(
            '~cmd_angle_instead_rotvel', False)
        self.wheelbase = rospy.get_param('~wheelbase', 1)

        self.robot = Botvac(self.port)

        rospy.Subscriber(self.cmd_vel_topic, Twist, self.cmdVelCb)
        #        self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher(self.odom_topic,
                                       Odometry,
                                       queue_size=10)
        self.distPub = rospy.Publisher(self.dist_topic,
                                       Vector3Stamped,
                                       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 spin(self):
        encoders = [0, 0]

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now() + rospy.Duration(1.0)

        # things that don't ever change
        #       scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        #       scan = LaserScan(header=rospy.Header(frame_id=scan_link))

        #       scan.angle_min =0.0
        #       scan.angle_max =359.0*pi/180.0
        #       scan.angle_increment =pi/180.0
        #       scan.range_min = 0.020
        #       scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
                        child_frame_id=self.base_frame)

        dist = Vector3Stamped(header=rospy.Header(frame_id=self.odom_frame))

        #        button = Button()
        #        sensor = Sensor()
        self.robot.setBacklight(1)
        self.robot.setLED("Green")
        # main loop of driver
        r = rospy.Rate(10)
        cmd_rate = self.CMD_RATE

        self.prevRanges = []

        while not rospy.is_shutdown():
            # notify if low batt
            #if self.robot.getCharger() < 10:
            #    print "battery low " + str(self.robot.getCharger()) + "%"
            # get motor encoder values
            left, right = self.robot.getMotors()

            cmd_rate = cmd_rate - 1
            if cmd_rate == 0:
                # send updated movement commands
                #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
                # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
                #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
                self.robot.setMotors(
                    self.cmd_vel[0], self.cmd_vel[1],
                    max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
                cmd_rate = self.CMD_RATE

            self.old_vel = self.cmd_vel

            # prepare laser scan
            #           scan.header.stamp = rospy.Time.now()

            #           self.robot.requestScan()
            #           scan.ranges = self.robot.getScanRanges()

            #            # now update position information
            #            dt = (scan.header.stamp - then).to_sec()
            #            then = scan.header.stamp

            # compute delta time
            dt = (rospy.Time.now() - then).to_sec()
            then = rospy.Time.now()

            d_left = (left - encoders[0]) / 1000.0
            d_right = (right - encoders[1]) / 1000.0
            encoders = [left, right]
            #print d_left, d_right, encoders

            dx = (d_left + d_right) / 2
            dth = (d_right - d_left) / (self.robot.base_width / 1000.0)

            x = cos(dth) * dx
            y = -sin(dth) * dx
            self.x += cos(self.th) * x - sin(self.th) * y
            self.y += sin(self.th) * x + cos(self.th) * y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # prepare odometry
            # synchronize /odom and /scan in time
            #odom.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
            # no need to synchronize /odom and /scan
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx / dt
            odom.twist.twist.angular.z = dth / dt

            dist.header.stamp = odom.header.stamp
            dist.vector.x = encoders[0] / 1000
            dist.vector.y = encoders[1] / 1000

            # sensors
            #            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
            #
            #            # buttons
            #            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()
            #
            #
            # publish everything
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
                self.base_frame, self.odom_frame)

            #            if self.prevRanges != scan.ranges:
            #                self.scanPub.publish(scan)
            #                self.prevRanges = scan.ranges

            self.odomPub.publish(odom)
            self.distPub.publish(dist)

            #            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            #            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            #            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
            #                if b == 1:
            #                    button.value = b
            #                    button.name = button_enum[idx]
            #                    self.buttonPub.publish(button)
            #
            #            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
            #                if b == 1:
            #                    sensor.value = b
            #                    sensor.name = sensor_enum[idx]
            #                    self.sensorPub.publish(sensor)

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setBacklight(0)
        self.robot.setLED("Off")
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def sign(self, a):
        if a >= 0:
            return 1
        else:
            return -1

    def convertTransSteeringAngleToRotVel(self, v, angle, wheelbase):
        return tan(angle) * v / wheelbase

    def cmdVelCb(self, req):
        if self.cmd_angle_instead_rotvel:
            req.angular.z = convertTransSteeringAngleToRotVel(
                req.linear.x, req.angular.z, self.wheelbase)

        x = req.linear.x * 1000
        th = req.angular.z * (self.robot.base_width / 2)
        k = max(abs(x - th), abs(x + th))
        # sending commands higher than max speed will fail

        if k > self.robot.max_speed:
            x = x * self.robot.max_speed / k
            th = th * self.robot.max_speed / k

        self.cmd_vel = [int(x - th), int(x + th)]