Пример #1
0
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)
        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False
Пример #2
0
    def __init__(self):
        self.chairbot_number = chairbot_number()
        self.teleop_node_name = 'teleop' + self.chairbot_number
        self.chairmovement_topic_name = 'chairMovement' + self.chairbot_number
        self.cbon_topic_name = 'cbon' + self.chairbot_number
        
        rospy.init_node(self.teleop_node_name)

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

        self.robot = Botvac(self.port)
    
        rospy.loginfo("Object is successfully init")
        rospy.Subscriber(self.chairmovement_topic_name, Twist, self.twist_handler, queue_size=10)
        rospy.Subscriber(self.cbon_topic_name, Int8, self.cbon, queue_size=10)
        self.latest_twist = Twist() # crete an empty twist packet
    
        self.SPEED = 150
        self.DIST = 20
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.status = False
Пример #3
0
    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]
Пример #4
0
    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
Пример #5
0
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_vel = [0, 0]
Пример #6
0
    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
Пример #7
0
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop03', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy03", Joy, self.joy_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        '''
Пример #8
0
    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('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
Пример #9
0
def readCommandsFromNeato():
    port = rospy.get_param('~port', "/dev/ttyACM0")
    robot = Botvac(port)
    doc = ""
    commands = robot.getAllCommands()
    for c in commands:
        com = robot.getCommandDescription(c)
        doc += "### " + com.command + "\n\n"
        doc += "**Description:** " + com.description  + "\n\n"
        if len(com.arguments) > 0:
            doc += "**Options:** \n\n| Flag | Description |\n|------|-------------|\n"
            for arg in com.arguments:
                doc += "| " + arg + " | "  + com.arguments[arg] + " |\n"
            doc += "\n\n"
    robot.exit()
    return doc
Пример #10
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop03', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy03", Joy, self.joy_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        '''
	def optiona(self):
	  #forward
	  self.SPEED = 300
	  #right
	  self.robot.setMotors(-50,50,self.SPEED)
	  rospy.sleep(1)
	  #left
	  self.robot.setMotors(100,-100,self.SPEED)
	  rospy.sleep(1)
	  self.robot.setMotors(-50,50,self.SPEED)
	  rospy.sleep(1)
	def optionb(self):
	  self.SPEED=100
	  self.robot.setMotors(-100,-100,self.SPEED)
	  rospy.sleep(1)
	  self.robot.setMotors(100,100,self.SPEED)
	  rospy.sleep(1)
	def optionc(self):
	  self.SPEED=200
	  self.robot.setMotors(-100,-100,self.SPEED/2)
	  rospy.sleep(1)
	  self.robot.setMotors(200,200,self.SPEED*(1.5))
	  rospy.sleep(1)
	def optiond(self):
	  self.SPEED=200
	  for _ in range(2):
	    self.robot.setMotors(-100,-100,self.SPEED)
	    rospy.sleep(1)
	    self.robot.setMotors(100,100,self.SPEED)
	    rospy.sleep(1)
	'''

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330
            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            print(x, y, self.SPEED)
            self.robot.setMotors(x, y, self.SPEED)
            self.robot.flushing()
            '''
            if tr == 1:
    			self.optiona()
			if ci == 1:
				self.optionb()
			if xx == 1:
				self.optionc()
			if sq == 1:
				self.optiond()
            '''
            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes
Пример #11
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('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', 'laser_frame')
        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_link')

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

            # send updated movement commands
            self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))

            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


            # publish everything
            self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z,
                                                                     quaternion.w), then, "base_link", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)

            r.sleep()

        # shut down
        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)]
Пример #12
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 150
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False
        self.touch9 = False
        self.touch10 = False
        self.touch11 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            leftButt = self.Butt[4]
            rightButt = self.Butt[5]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            # CHECK IF CONTROLLER BUTTONS ARE PRESSED
            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()
            if leftButt == 1:
                self.optione()
            if rightButt == 1:
                self.optionf()

            # CHECK IF TOUCH SENSORS ARE PRESSED
            if (self.touch0 or self.touch1):
                pass
            else:
                if (self.touch2 or self.touch3):
                    self.fwd()
                if (self.touch4 or self.touch5):
                    self.back()
                if (self.touch6 or self.touch7):
                    self.right()
                if (self.touch8 or self.touch9):
                    self.left()
                if (self.touch10 or self.touch11):
                    self.slowFwd()

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # SQUARE
    def optiona(self):
        self.walk1Forward()

    # TRIANGLE
    def optionb(self):
        self.walk1Forward()

    # CIRCLE
    def optionc(self):
        self.walk1Forward()

    # CROSS
    def optiond(self):
        self.walk1Forward()

    # LEFT BUTTON
    def optione(self):
        self.walk1Forward()

    # RIGHT BUTTON
    def optionf(self):
        self.walk1Forward()

    # PROGRAMMED FUNCTIONS

    # TOUCH SENSOR FUNCTIONS
    def fwd(self):
        SPEED = 150
        self.robot.setMotors(-500, -500, SPEED)
        rospy.sleep(1.75)

    def slowFwd(self):
        SPEED = 75
        self.robot.setMotors(-100, -100, SPEED)
        rospy.sleep(1)

    def back(self):
        SPEED = 150
        self.robot.setMotors(500, 500, SPEED)
        rospy.sleep(1.75)

    def right(self):
        SPEED = 150
        self.robot.setMotors(200, -200, SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED = 150
        self.robot.setMotors(-200, 200, SPEED)
        rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        if on.data == 1:
            pass
        elif on.data == 0:
            pass

    def touch_handler(self, msg):
        self.touch_number = msg.data
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = True
        if self.touch_number == 7:
            self.touch7 = True
        if self.touch_number == 8:
            self.touch8 = True
        if self.touch_number == 9:
            self.touch9 = True
        if self.touch_number == 10:
            self.touch10 = True
        if self.touch_number == 11:
            self.touch11 = True

        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 18:
            self.touch5 = False
        if self.touch_number == 19:
            self.touch7 = False
        if self.touch_number == 20:
            self.touch8 = False
        if self.touch_number == 21:
            self.touch9 = False
        if self.touch_number == 22:
            self.touch10 = False
        if self.touch_number == 23:
            self.touch11 = False
Пример #13
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, "")
Пример #14
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)]
Пример #15
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)]
Пример #16
0
class NeatoNode:

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 150
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            leftButt = self.Butt[4]
            rightButt = self.Butt[5]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR-1)*10)
            self.SPEED += ((AageL-1)*10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED<0):
                self.SPEED=0
            elif (self.SPEED>330):
                self.SPEED=330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d*self.DIST)
            rr = (Rgh_t*self.DIST)
            if (rr>=0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x=int(x)
            y=int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP<self.SPEEDSET):
            	self.SPEEDRAMP += (speeddif/20)
            else:
            	self.SPEEDRAMP -= (speeddif/20)

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0
            elif (self.SPEEDRAMP>330):
                self.SPEEDRAMP=330

            if (self.SPEEDSET > 150):
                if (0<x<10):
                    x=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<x<0):
                    x=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150

                if (0<y<10):
                    y=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<y<0):
                    y=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
            else:
                if (0<x<5):
                    x=5
                elif (-5<x<0):
                    x=-5

                if (0<y<5):
                    y=5
                elif (-5<y<0):
                    y=-5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
            	self.xramp += 1
            elif (self.xramp == self.lastx):
            	pass
            else:
            	self.xramp -= 1

            if (self.yramp < self.lasty):
            	self.yramp += 1
            elif (self.yramp == self.lasty):
            	pass
            else:
            	self.yramp -= 1


            if (x==0 and y==0):
            	self.SPEEDRAMP -= (self.SPEEDSET/10)
            else:
                if ((abs(self.xramp-x)>20) or (abs(self.yramp-y)>20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0

            self.lastx = x
            self.lasty = y
            print (self.xramp, x, self.lastx, self.yramp, y, self.lasty, self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()
            if leftButt == 1:
                self.optione()
            if rightButt == 1:
                self.optionf()

            if (self.touch6):
                pub_LED.publish(1)
            else:
                pub_LED.publish(0)
            if (self.touch0):
                self.right()
            if (self.touch1):
                self.fwd()
            if (self.touch2):
                self.left()
            if (self.touch3):
                self.back()

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # TRIANGLE  -   GREET
    def optionb(self):
        self.lightup()
        self.turn360Left()
        rospy.sleep(0.5)
        self.turn180Left()

    # CIRCLE    -   BRING TO TABLE 1
    def optionc(self):
        self.walk10Forward()
        self.walk3Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn180Right()

    # CROSS     -   BRING TO TABLE 2
    def optiond(self):
        self.turn45Right()
        self.walk10Forward()
        self.walk10Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn180Right()

    # SQUARE    -   BRING TO TABLE 1 / OFFER UP
    def optiona(self):
        self.walk10Forward()
        self.walk3Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn270Right()
        self.walk3Backward()

    # LEFT BUTTON   -   RETURN TO STATION FROM TABLE 1
    def optione(self):
        self.walk10Forward()
        self.walk3Forward()

    # RIGHT BUTTON  -   RETURN TO STATION FROM TABLE 2
    def optionf(self):
        self.walk10Forward()
        self.walk10Forward()
        self.turn45Left()

    # WALK FORWARD
    def walk1Forward(self):
        SPEED=200
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)

    def walk3Forward(self):
        SPEED=200
        self.robot.setMotors(-300,-300,SPEED)
        rospy.sleep(2)
        
    def walk5Forward(self):
        SPEED=200
        self.robot.setMotors(-500,-500,SPEED)
        rospy.sleep(3)

    def walk10Forward(self):
        SPEED=200
        self.robot.setMotors(-1200,-1200,SPEED)
        rospy.sleep(6)

    # WALK BACKWARD
    def walk1Backward(self):
        SPEED=200
        self.robot.setMotors(100,100,SPEED)
        rospy.sleep(1)

    def walk3Backward(self):
        SPEED=200
        self.robot.setMotors(300,300,SPEED)
        rospy.sleep(2)
        
    def walk5Backward(self):
        SPEED=200
        self.robot.setMotors(500,500,SPEED)
        rospy.sleep(3)

    def walk10Backward(self):
        SPEED=200
        self.robot.setMotors(1200,1200,SPEED)
        rospy.sleep(6)

    # TURN LEFT
    def turn45Left(self):
        SPEED=200
        self.robot.setMotors(-110,110,SPEED)
        rospy.sleep(1)

    def turn90Left(self):
        SPEED=200
        self.robot.setMotors(-200,200,SPEED)
        rospy.sleep(1.60)

    def turn180Left(self):
        SPEED=200
        self.robot.setMotors(-400,400,SPEED)
        rospy.sleep(3)

    def turn270Left(self):
        SPEED=200
        self.robot.setMotors(-605,605,SPEED)
        rospy.sleep(4.25)

    def turn360Left(self):
        SPEED=200
        self.robot.setMotors(-805,805,SPEED)
        rospy.sleep(5)

    def turn450Left(self):
        SPEED=200
        self.robot.setMotors(-1005,1005,SPEED)
        rospy.sleep(6.5)

    # TURN RIGHT
    def turn45Right(self):
        SPEED=200
        self.robot.setMotors(120,-120,SPEED)
        rospy.sleep(1)

    def turn90Right(self):
        SPEED=200
        self.robot.setMotors(225,-225,SPEED)
        rospy.sleep(1.75)

    def turn180Right(self):
        SPEED=200
        self.robot.setMotors(425,-425,SPEED)
        rospy.sleep(3)

    def turn270Right(self):
        SPEED=200
        self.robot.setMotors(625,-625,SPEED)
        rospy.sleep(4.25)

    def turn360Right(self):
        SPEED=200
        self.robot.setMotors(825,-825,SPEED)
        rospy.sleep(5)

    def turn450Right(self):
        SPEED=200
        self.robot.setMotors(1025,-1025,SPEED)
        rospy.sleep(6.5)



    def shakeNo(self):
        SPEED=275
        self.robot.setMotors(-120,120,SPEED)
        rospy.sleep(1.15)
        self.robot.setMotors(235,-235,SPEED)
        rospy.sleep(1.50)
        self.robot.setMotors(-210,210,SPEED)
        rospy.sleep(1.25)
        self.robot.setMotors(120,-120,SPEED)
        rospy.sleep(1.15)

    def lightup(self):
        pub_LED.publish(1)
        rospy.sleep(0.5)
        pub_LED.publish(0)
        rospy.sleep(0.5)
        pub_LED.publish(1)
        rospy.sleep(0.5)
        pub_LED.publish(0)
        rospy.sleep(0.5)

    # TOUCH SENSOR FUNCTIONS
    def fwd(self):
        SPEED=150
        self.robot.setMotors(-500,-500,SPEED)
        rospy.sleep(1.75)

    def back(self):
        SPEED=150
        self.robot.setMotors(500,500,SPEED)
        rospy.sleep(1.75)

    def right(self):
        SPEED=150
        self.robot.setMotors(200,-200,SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED=150
        self.robot.setMotors(-200,200,SPEED)
        rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt =  ps.buttons
        self.Axess = ps.axes

    def touch_handler(self, msg):
        self.touch_number = msg.data
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = not self.touch6
        if self.touch_number == 7:
            self.touch7 = True
        if self.touch_number == 8:
            self.touch8 = True

        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 19:
            self.touch7 = False
        if self.touch_number == 20:
            self.touch8 = False
Пример #17
0
class NeatoNode:

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)
        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):        
        
        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR-1)*10)
            self.SPEED += ((AageL-1)*10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED<0):
                self.SPEED=0
            elif (self.SPEED>330):
                self.SPEED=330
            
            self.SPEEDSET = self.SPEED

            ll = (Lft_d*self.DIST)
            rr = (Rgh_t*self.DIST)
            if (rr>=0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr) 

            x=int(x)
            y=int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP<self.SPEEDSET):
                self.SPEEDRAMP += (speeddif/20)
            else:
                self.SPEEDRAMP -= (speeddif/20)

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0
            elif (self.SPEEDRAMP>330):
                self.SPEEDRAMP=330

            if (self.SPEEDSET > 150):
                if (0<x<10):
                    x=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<x<0):
                    x=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150

                if (0<y<10):
                    y=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<y<0):
                    y=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
            else:
                if (0<x<5):
                    x=5
                elif (-5<x<0):
                    x=-5

                if (0<y<5):
                    y=5
                elif (-5<y<0):
                    y=-5
            
            #self.xramp = x 
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1


            if (x==0 and y==0):
                self.SPEEDRAMP -= (self.SPEEDSET/10)
            else:
                if ((abs(self.xramp-x)>20) or (abs(self.yramp-y)>20)):
                    self.SPEEDRAMP = 50
            
            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0

            self.lastx = x
            self.lasty = y
            print (self.xramp, x, self.lastx, self.yramp, y, self.lasty, self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

            if (self.touch6):
                pub_LED.publish(1)
                if (self.touch0 or self.touch1):
                    self.left()
                if (self.touch3 or self.touch2):
                    self.right()
                if (self.touch4):
                    self.back()
                if (self.touch5):
                    self.fwd()
            elif (self.touch7):
                pub_LED.publish(1)
                if (self.touch0 or self.touch1):
                    self.leftFast()
                if (self.touch3 or self.touch2):
                    self.rightFast()
                if (self.touch4):
                    self.backFast()
                if (self.touch5):
                    self.fwdFast()
            else:
                pub_LED.publish(0)

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off") 

    # SQUARE
    def optiona(self):
        SPEED = 125

        self.robot.setMotors(-420,420,SPEED)
        rospy.sleep(3.5)

        self.robot.setMotors(50,50,SPEED)
        rospy.sleep(1)

    # TRIANGLE
    def optionb(self):  
        SPEED=100
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)
        self.robot.setMotors(100,100,SPEED)
        rospy.sleep(1)

    # CIRCLE
    def optionc(self):  
        SPEED=200
        self.robot.setMotors(-100,-100,SPEED/2)
        rospy.sleep(1)
        self.robot.setMotors(200,200,SPEED*(1.5))
        rospy.sleep(1)

    # X
    def optiond(self):  
        SPEED=200
        for _ in range(2):
            self.robot.setMotors(-100,-100,SPEED)
            rospy.sleep(1)
            self.robot.setMotors(100,100,SPEED)
            rospy.sleep(1)

    def fwdFast(self):
        SPEED=300
        self.robot.setMotors(-300,-300,SPEED)
        rospy.sleep(1)

    def backFast(self):
        SPEED=200
        self.robot.setMotors(200,200,SPEED)
        rospy.sleep(1)

    def rightFast(self):
        SPEED=150
        self.robot.setMotors(150,-150,SPEED)
        rospy.sleep(1)

    def leftFast(self):
        SPEED=150
        self.robot.setMotors(-150,150,SPEED)
        rospy.sleep(1)

    def fwd(self):
        SPEED=100
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)

    def back(self):
        SPEED=50
        self.robot.setMotors(50,50,SPEED)
        rospy.sleep(1) 

    def right(self):
        SPEED=50
        self.robot.setMotors(50,-50,SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED=50
        self.robot.setMotors(-50,50,SPEED)
        rospy.sleep(1)

    def turnRight(self):
        SPEED=100
        self.robot.setMotors(220,-220,150)
        rospy.sleep(2.25)

    def turnLeft(self):
        SPEED=100
        self.robot.setMotors(-210,210,SPEED)
        rospy.sleep(2.25)

    def stop(self):
        SPEED=00
        self.robot.setMotors(00,00,SPEED)
        rospy.sleep(1)
        self.robot.setMotors(00,00,SPEED)

    def joy_handler(self, ps):
        self.Butt =  ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        pub_LED.publish(on.data)
        print(on.data)
        if on.data == 1:
            self.touch6=True
        elif on.data == 0:
            self.touch6=False

    def touch_handler(self, msg):
        self.touch_number = msg.data
        print self.touch_number
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = not self.touch6
            if self.touch7 == True:
                self.touch7 = False
        if self.touch_number == 7:
            self.touch7 = not self.touch7
            if self.touch6 == True:
                self.touch6 = False
        if self.touch_number == 8:
            self.touch8 = True
        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 20:
            self.touch8 = False
Пример #18
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")
        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('soft_button', Button, queue_size=100)
        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 = -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')

        softb = Button()

        # main loop of driver
        r = rospy.Rate(5)
        while not rospy.is_shutdown():

            # get motor encoder values
            left, right = self.robot.getMotors()

            # send updated movement commands
            if self.cmd_vel != self.old_vel:
                self.robot.setMotors(
                    self.cmd_vel[0], self.cmd_vel[1],
                    max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))

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

            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]

            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)

            # 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(
            )
            softb.value = btn_soft
            softb.name = "Soft_Button"

            # publish everything
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
                "base_link", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)
            self.buttonPub.publish(softb)

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

        # shut down
        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)]
Пример #19
0
class NeatoNode:

    def __init__(self):
        self.chairbot_number = chairbot_number()
        self.teleop_node_name = 'teleop' + self.chairbot_number
        self.chairmovement_topic_name = 'chairMovement' + self.chairbot_number
        self.cbon_topic_name = 'cbon' + self.chairbot_number
        
        rospy.init_node(self.teleop_node_name)

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

        self.robot = Botvac(self.port)
    
        rospy.loginfo("Object is successfully init")
        rospy.Subscriber(self.chairmovement_topic_name, Twist, self.twist_handler, queue_size=10)
        rospy.Subscriber(self.cbon_topic_name, Int8, self.cbon, queue_size=10)
        self.latest_twist = Twist() # crete an empty twist packet
    
        self.SPEED = 150
        self.DIST = 20
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.status = False

    def spin(self):
        # main loop of driver
        r = rospy.Rate(20)
        DIRECTION = 1 # 1 for forward, -1 for backwards
        while not rospy.is_shutdown():
            z = self.latest_twist.angular.z
            x = self.latest_twist.linear.x
            if x == 0 and z == 0: #we were told not to move at all
                SPEED = 0
                dist_l = 0
                dist_r = 0
                #keepalive action
                # print("keepalive action fired")
                self.robot.requestScan()
            else:
                if x != 0:
                    SPEED = abs(x)   
                    if x < 0:
                        DIRECTION = -1 # backwards
                    else:
                        DIRECTION = 1 # forwards
                else:
                    SPEED = 150

                print(self.latest_twist)

                if z != 0:
                    dist_l = -int(self.DIST*z)
                    dist_r = int(self.DIST*z)
                else:
                    dist_l = int(DIRECTION*self.DIST)
                    dist_r = int(DIRECTION*self.DIST)

            if(self.status):
                self.robot.setMotors(dist_l, dist_r, SPEED)
            else:
                print("Chair #" + self.chairbot_number + " is OFF")
            
            r.sleep()
            # wait, then do it again

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def twist_handler(self, twist_msg):
        print('twist handler')
        self.latest_twist = twist_msg

    def cbon(self, on):
        if on.data == 1:
            self.status = True
        elif on.data == 0:
            self.status = False
        print("You are driving chair " + self.chairbot_number)
Пример #20
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('neato')

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.scanPub = rospy.Publisher('scan', LaserScan, queue_size=10)
        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.odomBroadcaster = TransformBroadcaster()

        self.cmd_vel = [0, 0]

    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
        scan.angle_max = 6.26
        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_footprint')

        # main loop of driver
        r = rospy.Rate(10)
        self.robot.requestScan()
        while not rospy.is_shutdown():
            # prepare laser scan
            scan.header.stamp = rospy.Time.now()
            #self.robot.requestScan()
            scan.ranges = self.robot.getScanRanges()

            # get motor encoder values
            left, right = self.robot.getMotors()

            # send updated movement commands
            self.robot.setMotors(
                self.cmd_vel[0], self.cmd_vel[1],
                max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))

            # ask for the next scan while we finish processing stuff
            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]

            dx = (d_left + d_right) / 2
            dth = (d_right - d_left) / (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)

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

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def cmdVelCb(self, req):
        x = req.linear.x * (1000)
        th = req.angular.z * (BASE_WIDTH / 2)
        k = max(abs(x - th), abs(x + th))
        # sending commands higher than max speed will fail
        if k > MAX_SPEED:
            x = x * MAX_SPEED / k
            th = th * MAX_SPEED / k
        self.cmd_vel = [int(x - th), int(x + th)]
Пример #21
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def optiona(self):
        #forward
        SPEED = 300
        #right
        self.robot.setMotors(-50, 50, SPEED)
        rospy.sleep(1)
        #left
        self.robot.setMotors(100, -100, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(-50, 50, SPEED)
        rospy.sleep(1)

    def optionb(self):
        SPEED = 100
        self.robot.setMotors(-100, -100, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(100, 100, SPEED)
        rospy.sleep(1)

    def optionc(self):
        SPEED = 200
        self.robot.setMotors(-100, -100, SPEED / 2)
        rospy.sleep(1)
        self.robot.setMotors(200, 200, SPEED * (1.5))
        rospy.sleep(1)

    def optiond(self):
        SPEED = 200
        for _ in range(2):
            self.robot.setMotors(-100, -100, SPEED)
            rospy.sleep(1)
            self.robot.setMotors(100, 100, SPEED)
            rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes
Пример #22
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

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

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        # rospy.Subscriber('/touches02', Int8, self.touch_handler, queue_size=10)
        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

            # if (self.touch6):
            # 	pub_LED.publish(1)
            # 	if (self.touch0 or self.touch1):
            # 		self.left()
            # 	if (self.touch3 or self.touch2):
            # 		self.right()
            # 	if (self.touch4):
            # 		self.back()
            # 	if (self.touch5):
            # 		self.fwd()
            # elif (self.touch7):
            # 	pub_LED.publish(1)
            # 	if (self.touch0 or self.touch1):
            # 		self.leftFast()
            # 	if (self.touch3 or self.touch2):
            # 		self.rightFast()
            # 	if (self.touch4):
            # 		self.backFast()
            # 	if (self.touch5):
            # 		self.fwdFast()
            # else:
            # 	pub_LED.publish(0)

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

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # SQUARE
    def optiona(self):
        self.lightup(1, 3)
        self.lightup(0, 1)
        self.lightup(1, 1)
        self.lightup(0, 1)
        self.lightup(1, 1)
        self.lightup(0, 1)
        self.lightup(1, 1)

        self.lightup(0, 1)

    # TRIANGLE
    def optionb(self):
        # self.right(325,300)# almost 120 degree
        # self.fwd()
        BEET = .98
        #Enter

        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)

        #03 forward
        self.fwd(0, 0, 7 * BEET)
        self.fwd(0, 0, BEET)

        # shake
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)

        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        #Turn pause, turn pause
        self.left(400, 220, 3 * BEET)
        self.fwd(0, 0, 0.05 * BEET)
        self.left(600, 250, BEET)
        self.fwd(0, 0, 0.1 * BEET)

        # self.lightup(1,BEET)
        # self.lightup(0,0.5*BEET)
        # self.lightup(1,BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.right(300, 120, 3 * BEET)
        self.fwd(0, 0, BEET)

        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.fwd(0, 0, 0.5 * BEET)

        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.left(300, 100, 2 * BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.right(600, 250, 2 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.left(2200, 150, 12 * BEET)
        self.fwd(0, 0, 2 * BEET)

        #Thriller
        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, BEET)
        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)
        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        #Thriller
        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.right(100, 200, 0.5 * BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, 2 * BEET)

        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.fwd(0, 0, 2 * BEET)
        self.left(600, 250, BEET)

        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

    # CIRCLE IS FOR TESTING
    def optionc(self):
        BEET = .98
        #Enter
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)

    # X
    def optiond(self):
        self.lightup()

    def fwd(self, DISTANCE, SPEED, SLEEP):
        self.robot.setMotors(-DISTANCE, -DISTANCE, SPEED)
        rospy.sleep(SLEEP)

    def back(self, DISTANCE, SPEED, SLEEP):
        self.robot.setMotors(DISTANCE, DISTANCE, SPEED)
        rospy.sleep(SLEEP)

    def right(self, ANGLE, SPEED, SLEEP):
        self.robot.setMotors(ANGLE, -ANGLE, SPEED)
        rospy.sleep(SLEEP)

    def left(self, ANGLE, SPEED, SLEEP):
        self.robot.setMotors(-ANGLE, ANGLE, SPEED)
        rospy.sleep(SLEEP)

    def stop(self):
        SPEED = 00
        self.robot.setMotors(00, 00, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(00, 00, SPEED)

    def lightup(self, SWITCH, BEET):
        pub_LED.publish(SWITCH)
        rospy.sleep(BEET)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        pub_LED.publish(on.data)
        print(on.data)
        if on.data == 1:
            self.touch6 = True
        elif on.data == 0:
            self.touch6 = False