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 __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 __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 __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 __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 __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.CMD_RATE =2
self.newTwistCmd = 0
#self.port = rospy.get_param('~port', "/dev/ttyUSB0")
#self.telnet_ip = rospy.get_param('~telnet_ip', "192.168.1.107")
self.telnet_ip = rospy.get_param('~telnet_ip', "Huey-Tiger")
#rospy.loginfo("Using port: %s" % self.port)
rospy.loginfo("Using telnet: %s" % self.telnet_ip)
#self.robot = Botvac(self.port)
#self.robot = Huey(self.telnet_ip)
self.robot = Botvac(self.telnet_ip)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.buttonPub = rospy.Publisher('button', Button, queue_size=10)
self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0, 0]
self.old_vel = self.cmd_vel
def __init__(self):
""" 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 __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 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
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
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)]
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
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, "")
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)]
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)]
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
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
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)]
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)
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)]
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
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
