def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self._goal_x = 0
self._goal_y = 0
self._pose_x = 0
self._pose_y = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._quaternion = [0, 0, 0, 0]
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 0
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('controller' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato")
rospy.loginfo("Using port: %s" % (self._port))
self.ramp_rate = rospy.get_param("~ramp_rate", 0.3)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self._robot = Botvac(self._port)
#self.pub_motor = rospy.Publisher('roboBaseSub', NeatoCommand, queue_size=10)
self.rate = rospy.get_param("~rate", 20)
self.w = rospy.get_param("~base_width", 0.49)
self.ramping_enable = rospy.get_param("~ramping_enable", False)
self.velfactor = rospy.get_param("~mps_to_rpm", 1)
self.prev_left_vel = 0
self.prev_right_vel = 0
rospy.Subscriber('/neato01/cmd_vel_mux/input/navi', Twist,
self.twistCallback)
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/joy" + chairbot_number,
Joy,
self.joy_handler,
queue_size=10)
rospy.Subscriber("/cbon" + chairbot_number,
Int8,
self.cbon,
queue_size=10)
rospy.Subscriber('/touches' + chairbot_number,
Int8,
self.touch_handler,
queue_size=10)
rospy.Subscriber('/touches',
Int8,
self.global_touch_handler,
queue_size=10)
self._global_touch = rospy.Publisher('/touches', Int8, queue_size=10)
self._joystick_axes = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
self._joystick_buttons = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 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):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/joy" + chairbot_number,
Joy,
self.joy_handler,
queue_size=10)
self._joystick_axes = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
self._joystick_buttons = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 0
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/joy" + chairbot_number,
Joy,
self.joy_handler,
queue_size=10)
rospy.Subscriber("/neato08/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self._joystick_axes = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
self._joystick_buttons = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self._goal_x = 0
self._goal_y = 0
self._pose_x = 0
self._pose_y = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._quaternion = [0, 0, 0, 0]
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 0
# 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 = 175
self._robot.setMotors(-100, -100, SPEED)
rospy.sleep(1)
def fwd(self):
SPEED = 150
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(0.2)
def back(self):
SPEED = 150
self._robot.setMotors(200, 200, SPEED)
rospy.sleep(1)
def right(self):
SPEED = 150
self._robot.setMotors(150, -150, SPEED)
rospy.sleep(1)
def left(self):
SPEED = 50
self._robot.setMotors(-150, 150, SPEED)
rospy.sleep(1)
def turnRight(self):
SPEED = 200
self._robot.setMotors(220, -220, SPEED)
rospy.sleep(0.05)
def turnLeft(self):
SPEED = 200
self._robot.setMotors(-210, 210, SPEED)
rospy.sleep(0.05)
def stop(self):
SPEED = 00
self._robot.setMotors(00, 00, SPEED)
rospy.sleep(1)
self._robot.setMotors(00, 00, SPEED)
def eat(self):
self._pub.publish("Eating!")
SPEED = 200
self._robot.setMotors(-210, 210, SPEED)
rospy.sleep(0.25)
self._robot.setMotors(210, -210, SPEED)
rospy.sleep(0.25)
self._robot.setMotors(-210, 210, SPEED)
pub_linear.publish(1)
rospy.sleep(0.25)
self._robot.setMotors(210, -210, SPEED)
pub_linear.publish(2)
rospy.sleep(0.25)
self._robot.setMotors(-210, 210, SPEED)
pub_linear.publish(1)
rospy.sleep(0.25)
self._robot.setMotors(210, -210, SPEED)
pub_linear.publish(2)
rospy.sleep(0.25)
self._robot.setMotors(-210, 210, SPEED)
pub_linear.publish(1)
rospy.sleep(0.25)
self._robot.setMotors(210, -210, SPEED)
pub_linear.publish(2)
rospy.sleep(0.25)
pub_linear.publish(3)
SPEED = 0
self._robot.setMotors(0, 0, SPEED)
rospy.sleep(3)
def joy_handler(self, ps):
self._joystick_buttons = ps.buttons
self._joystick_axes = ps.axes
# Get goal
def goal_handler(self, data):
self._goal_x = data.pose.position.x
self._goal_y = data.pose.position.y
# Get goal
def pose_handler(self, data):
self._pose_x = data.pose.position.x
self._pose_y = data.pose.position.y
self._qx = data.pose.orientation.x
self._qy = data.pose.orientation.y
self._qz = data.pose.orientation.z
self._qw = data.pose.orientation.w
self._quaternion = data.pose.orientation
def follow(self):
goal_angle = atan2(self._goal_y - self._pose_y,
self._goal_x - self._pose_x)
current_angle = tf.transformations.euler_from_quaternion(
[self._qx, self._qy, self._qz, self._qw])[2]
#self._pub.publish("goal: " +str(goal_angle))
#self._pub.publish("current: " +str(current_angle))
ang_error = goal_angle - current_angle
pos_error = sqrt((self._goal_x - self._pose_x) *
(self._goal_x - self._pose_x) +
(self._goal_y - self._pose_y) *
(self._goal_y - self._pose_y))
if ang_error > 0:
if abs(ang_error) > 0.2:
self.turnLeft()
self._pub.publish("angle error: " + str(ang_error))
else:
if abs(ang_error) > 0.2:
self.turnRight()
self._pub.publish("angle error: " + str(ang_error))
if abs(ang_error) <= 0.2:
if pos_error > 0.8:
self.fwd()
self._pub.publish("pose error: " + str(pos_error))
else:
self.eat()
def run_away(self):
goal_angle = atan2(self._goal_y - self._pose_y,
self._goal_x - self._pose_x)
current_angle = tf.transformations.euler_from_quaternion(
[self._qx, self._qy, self._qz, self._qw])[2]
#self._pub.publish("goal: " +str(goal_angle))
#self._pub.publish("current: " +str(current_angle))
ang_error = goal_angle - current_angle
pos_error = sqrt((self._goal_x - self._pose_x) *
(self._goal_x - self._pose_x) +
(self._goal_y - self._pose_y) *
(self._goal_y - self._pose_y))
if ang_error > 0:
if abs(ang_error) > 0.3:
self.turnLeft()
self._pub.publish("error: " + str(ang_error))
else:
if abs(ang_error) > 0.3:
self.turnRight()
self._pub.publish("error: " + str(ang_error))
if abs(ang_error <= 0.3):
if pos_error < 0.9:
self.back()
def spin(self):
Lft_t = self._joystick_axes[0]
Lft_d = self._joystick_axes[1]
Rgh_t = self._joystick_axes[3]
Rgh_d = self._joystick_axes[4]
AageL = self._joystick_axes[2]
AageR = self._joystick_axes[5]
L_R = self._joystick_axes[6]
F_B = self._joystick_axes[7]
xx = self._joystick_buttons[0]
ci = self._joystick_buttons[1]
tr = self._joystick_buttons[2]
sq = self._joystick_buttons[3]
self._speed_s = self._joystick_buttons[4]
self._speed_f = self._joystick_buttons[5]
AageL_Button = self._joystick_buttons[6]
AageR_Button = self._joystick_buttons[7]
share = self._joystick_buttons[8]
options = self._joystick_buttons[9]
pressL = self._joystick_buttons[10]
pressR = self._joystick_buttons[11]
power = self._joystick_buttons[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._speed_set = self._speed
ll = (Lft_d * self._distance)
rr = (Rgh_t * self._distance)
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._speed_ramp - self._speed_set)
if (self._speed_ramp < self._speed_set):
self._speed_ramp += (speeddif / 20)
else:
self._speed_ramp -= (speeddif / 20)
if (self._speed_ramp < 0):
self._speed_ramp = 0
elif (self._speed_ramp > 330):
self._speed_ramp = 330
if (self._speed_set > 150):
if (0 < x < 10):
x = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < x < 0):
x = -10
if (self._speed_ramp > 150):
self._speed_ramp = 150
if (0 < y < 10):
y = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < y < 0):
y = -10
if (self._speed_ramp > 150):
self._speed_ramp = 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
if (self._x_ramp < self._last_x):
self._x_ramp += 1
elif (self._x_ramp == self._last_x):
pass
else:
self._x_ramp -= 1
if (self._y_ramp < self._last_y):
self._y_ramp += 1
elif (self._y_ramp == self._last_y):
pass
else:
self._y_ramp -= 1
if (x == 0 and y == 0):
self._speed_ramp -= (self._speed_set / 10)
else:
if ((abs(self._x_ramp - x) > 20) or (abs(self._y_ramp - y) > 20)):
self._speed_ramp = 50
if (self._speed_ramp < 0):
self._speed_ramp = 0
self._last_x = x
self._last_y = y
print(self._x_ramp, x, self._last_x, self._y_ramp, y, self._last_y,
self._speed_ramp, self._speed_set)
self._robot.setMotors(self._x_ramp, self._y_ramp, self._speed_ramp)
self._robot.flushing()
if tr == 1:
#self.optiona()
pub_linear.publish(1)
print('tr pressed')
if ci == 1:
#self.optionb()
pub_linear.publish(2)
if sq == 1:
self.run_away()
self._pub.publish("RUN AWAY!!")
if (xx == 0) and (sq == 0):
self.follow()
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
self.stop_dist = 0.75
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self.goal_x = 0
self.goal_y = 0
self.center_x = 0
self.center_y = 0
# ME #
self.orient = 0
self.pose_x = 0
self.pose_y = 0
# OTHERS #
self.posea = PoseStamped().pose.position
self.poseb = PoseStamped().pose.position
self.posec = PoseStamped().pose.position
self.dista = 10
self.distb = 10
self.distc = 10
# Formation center
#### HOW TO CHANGE FIDUICAL GOAL #####
### change neato#/pose
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
# Neatos
############# I AM NEATO 01 ##############################
# double check form of
if chairbot_number == '01':
self.i_am = 1
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '02':
self.i_am = 2
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '03':
self.i_am = 3
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '04':
self.i_am = 4
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
self.stop_dist = 0.75
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self.goal_x = 0
self.goal_y = 0
self.center_x = 0
self.center_y = 0
# ME #
self.orient = 0
self.pose_x = 0
self.pose_y = 0
# OTHERS #
self.posea = PoseStamped().pose.position
self.poseb = PoseStamped().pose.position
self.posec = PoseStamped().pose.position
self.dista = 10
self.distb = 10
self.distc = 10
# Formation center
#### HOW TO CHANGE FIDUICAL GOAL #####
### change neato#/pose
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
# Neatos
############# I AM NEATO 01 ##############################
# double check form of
if chairbot_number == '01':
self.i_am = 1
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '02':
self.i_am = 2
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '03':
self.i_am = 3
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '04':
self.i_am = 4
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
def fwd(self):
SPEED = 150
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(0.2)
def bck(self):
SPEED = 150
self._robot.setMotors(300, 300, SPEED)
rospy.sleep(0.2)
def turnRight(self):
SPEED = 200
self._robot.setMotors(200, -200, SPEED)
rospy.sleep(0.05)
def turnLeft(self):
SPEED = 200
self._robot.setMotors(-200, 200, SPEED)
rospy.sleep(0.05)
def stop(self):
SPEED = 00
self._robot.setMotors(00, 00, SPEED)
rospy.sleep(1)
self._robot.setMotors(00, 00, SPEED)
# Get goal
def goal_handler(self, data):
self.goal_x = data.pose.position.x
self.goal_y = data.pose.position.y
######## I AM THIS NEATO #############
def pose_handler(self, data):
self.pose_x = data.pose.position.x
self.pose_y = data.pose.position.y
qx = data.pose.orientation.x
qy = data.pose.orientation.y
qz = data.pose.orientation.z
qw = data.pose.orientation.w
self.orient = tf.transformations.euler_from_quaternion(
[qx, qy, qz, qw])[2]
#### THESE ARE THE OTHER NEATOES ####
def pose_handlera(self, data):
self.posea = data.pose.position
self.dista = sqrt((self.posea.x - self.pose_x)**2 +
(self.posea.y - self.pose_y)**2)
def pose_handlerb(self, data):
self.poseb = data.pose.position
self.distb = sqrt((self.poseb.x - self.pose_x)**2 +
(self.poseb.y - self.pose_y)**2)
def pose_handlerc(self, data):
self.posec = data.pose.position
self.distc = sqrt((self.posec.x - self.pose_x)**2 +
(self.posec.y - self.pose_y)**2)
def should_stop(self):
### WHO GETS PRIORITY ###
go = 1
if self.i_am == 1:
print("I am 1 and I yield to 2, 3, and the Queen")
if self.dista <= self.stop_dist or self.distb <= self.stop_dist or self.distc <= self.stop_dist:
go = 0
elif self.i_am == 2:
print("I am 2 and I yield to 3 and the Queen")
if self.distb <= self.stop_dist or self.distc <= self.stop_dist:
go = 0
elif self.i_am == 3:
print("I am 3 and I yield to the Queen")
if self.distc <= self.stop_dist:
go = 0
elif self.i_am == 4:
print("I am the Queen")
else:
print("She doesn't even go here")
return go
def follow(self):
goal_angle = atan2(self.goal_y - self.pose_y,
self.goal_x - self.pose_x)
current_angle = self.orient
ang_error = goal_angle - current_angle
pos_error = sqrt((self.goal_x - self.pose_x) *
(self.goal_x - self.pose_x) +
(self.goal_y - self.pose_y) *
(self.goal_y - self.pose_y))
### CHECK IF TOO CLOSE TO OTHER NEATOS ###
go = self.should_stop()
### ONLY MOVE IF NOT STOPPED BY OTHER NEATOS ###
if go == 1:
if ang_error > 0:
if abs(ang_error) > 0.2:
self.turnLeft()
# self._pub.publish("angle error: " +str(ang_error))
else:
if abs(ang_error) > 0.2:
self.turnRight()
# self._pub.publish("angle error: " +str(ang_error))
if abs(ang_error) <= 0.2:
if pos_error > self.stop_dist:
self.fwd()
self._pub.publish("pose01 error: " + str(pos_error))
else:
self.stop()
else:
self.stop()
def spin(self):
self._robot.flushing()
self.follow()
#self.run_away()
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/joy" + chairbot_number,
Joy,
self.joy_handler,
queue_size=10)
self._joystick_axes = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
self._joystick_buttons = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 0
# 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 = 175
self._robot.setMotors(-100, -100, SPEED)
rospy.sleep(1)
def fwd(self):
SPEED = 300
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(1)
def back(self):
SPEED = 200
self._robot.setMotors(200, 200, SPEED)
rospy.sleep(1)
def right(self):
SPEED = 150
self._robot.setMotors(150, -150, SPEED)
rospy.sleep(1)
def left(self):
SPEED = 150
self._robot.setMotors(-150, 150, SPEED)
rospy.sleep(1)
def turnRight(self):
SPEED = 100
self._robot.setMotors(220, -220, 330)
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._joystick_buttons = ps.buttons
self._joystick_axes = ps.axes
def spin(self):
Lft_t = self._joystick_axes[0]
Lft_d = self._joystick_axes[1]
Rgh_t = self._joystick_axes[3]
Rgh_d = self._joystick_axes[4]
AageL = self._joystick_axes[2]
AageR = self._joystick_axes[5]
L_R = self._joystick_axes[6]
F_B = self._joystick_axes[7]
sq = self._joystick_buttons[0]
xx = self._joystick_buttons[1]
ci = self._joystick_buttons[2]
tr = self._joystick_buttons[3]
self._speed_s = self._joystick_buttons[4]
self._speed_f = self._joystick_buttons[5]
AageL_Button = self._joystick_buttons[6]
AageR_Button = self._joystick_buttons[7]
share = self._joystick_buttons[8]
options = self._joystick_buttons[9]
pressL = self._joystick_buttons[10]
pressR = self._joystick_buttons[11]
power = self._joystick_buttons[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._speed_set = self._speed
ll = (Lft_d * self._distance)
rr = (Rgh_t * self._distance)
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._speed_ramp - self._speed_set)
if (self._speed_ramp < self._speed_set):
self._speed_ramp += (speeddif / 20)
else:
self._speed_ramp -= (speeddif / 20)
if (self._speed_ramp < 0):
self._speed_ramp = 0
elif (self._speed_ramp > 330):
self._speed_ramp = 330
if (self._speed_set > 330):
if (0 < x < 10):
x = 10
if (self._speed_ramp > 330):
self._speed_ramp = 330
elif (-10 < x < 0):
x = -10
if (self._speed_ramp > 330):
self._speed_ramp = 330
if (0 < y < 10):
y = 10
if (self._speed_ramp > 330):
self._speed_ramp = 330
elif (-10 < y < 0):
y = -10
if (self._speed_ramp > 330):
self._speed_ramp = 330
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
if (self._x_ramp < self._last_x):
self._x_ramp += 1
elif (self._x_ramp == self._last_x):
pass
else:
self._x_ramp -= 1
if (self._y_ramp < self._last_y):
self._y_ramp += 1
elif (self._y_ramp == self._last_y):
pass
else:
self._y_ramp -= 1
if (x == 0 and y == 0):
self._speed_ramp -= (self._speed_set / 10)
else:
if ((abs(self._x_ramp - x) > 20) or (abs(self._y_ramp - y) > 20)):
self._speed_ramp = 50
if (self._speed_ramp < 0):
self._speed_ramp = 0
self._last_x = x
self._last_y = y
print(self._x_ramp, x, self._last_x, self._y_ramp, y, self._last_y,
self._speed_ramp, self._speed_set)
self._robot.setMotors(self._x_ramp, self._y_ramp, self._speed_ramp)
self._robot.flushing()
if tr == 1:
self.optiona()
if ci == 1:
self.optionb()
if xx == 1:
self.optionc()
if sq == 1:
self.optiond()
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self._goal_x = 0
self._goal_y = 0
self._pose_x = 0
self._pose_y = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._quaternion = [0, 0, 0, 0]
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 0
def fwdFast(self):
SPEED = 175
self._robot.setMotors(-100, -100, SPEED)
rospy.sleep(1)
def fwd(self):
SPEED = 150
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(0.2)
def back(self):
SPEED = 150
self._robot.setMotors(200, 200, SPEED)
rospy.sleep(1)
def right(self):
SPEED = 150
self._robot.setMotors(150, -150, SPEED)
rospy.sleep(1)
def left(self):
SPEED = 50
self._robot.setMotors(-150, 150, SPEED)
rospy.sleep(1)
def turnRight(self):
SPEED = 200
self._robot.setMotors(220, -220, SPEED)
rospy.sleep(0.05)
def turnLeft(self):
SPEED = 200
self._robot.setMotors(-210, 210, SPEED)
rospy.sleep(0.05)
def stop(self):
SPEED = 00
self._robot.setMotors(00, 00, SPEED)
rospy.sleep(1)
self._robot.setMotors(00, 00, SPEED)
# Get goal
def goal_handler(self, data):
self._goal_x = data.pose.position.x
self._goal_y = data.pose.position.y
# Get goal
def pose_handler(self, data):
self._pose_x = data.pose.position.x
self._pose_y = data.pose.position.y
self._qx = data.pose.orientation.x
self._qy = data.pose.orientation.y
self._qz = data.pose.orientation.z
self._qw = data.pose.orientation.w
self._quaternion = data.pose.orientation
def follow(self):
goal_angle = atan2(self._goal_y - self._pose_y,
self._goal_x - self._pose_x)
current_angle = tf.transformations.euler_from_quaternion(
[self._qx, self._qy, self._qz, self._qw])[2]
#self._pub.publish("goal: " +str(goal_angle))
#self._pub.publish("current: " +str(current_angle))
ang_error = goal_angle - current_angle
pos_error = sqrt((self._goal_x - self._pose_x) *
(self._goal_x - self._pose_x) +
(self._goal_y - self._pose_y) *
(self._goal_y - self._pose_y))
if ang_error > 0:
if abs(ang_error) > 0.2:
self.turnLeft()
self._pub.publish("angle error: " + str(ang_error))
else:
if abs(ang_error) > 0.2:
self.turnRight()
self._pub.publish("angle error: " + str(ang_error))
if abs(ang_error) <= 0.2:
if pos_error > 0.8:
self.fwd()
self._pub.publish("pose error: " + str(pos_error))
else:
self.stop()
def run_away(self):
goal_angle = atan2(self._goal_y - self._pose_y,
self._goal_x - self._pose_x)
current_angle = tf.transformations.euler_from_quaternion(
[self._qx, self._qy, self._qz, self._qw])[2]
#self._pub.publish("goal: " +str(goal_angle))
#self._pub.publish("current: " +str(current_angle))
ang_error = goal_angle - current_angle
pos_error = sqrt((self._goal_x - self._pose_x) *
(self._goal_x - self._pose_x) +
(self._goal_y - self._pose_y) *
(self._goal_y - self._pose_y))
if ang_error > 0:
if abs(ang_error) > 0.3:
self.turnLeft()
self._pub.publish("error: " + str(ang_error))
else:
if abs(ang_error) > 0.3:
self.turnRight()
self._pub.publish("error: " + str(ang_error))
if abs(ang_error <= 0.3):
if pos_error < 0.9:
self.back()
def spin(self):
self._speed = 200
self._speed = int(self._speed)
if (self._speed < 0):
self._speed = 0
elif (self._speed > 330):
self._speed = 330
self._speed_set = self._speed
ll = 0
rr = 0
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._speed_ramp - self._speed_set)
if (self._speed_ramp < self._speed_set):
self._speed_ramp += (speeddif / 20)
else:
self._speed_ramp -= (speeddif / 20)
if (self._speed_ramp < 0):
self._speed_ramp = 0
elif (self._speed_ramp > 330):
self._speed_ramp = 330
if (self._speed_set > 150):
if (0 < x < 10):
x = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < x < 0):
x = -10
if (self._speed_ramp > 150):
self._speed_ramp = 150
if (0 < y < 10):
y = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < y < 0):
y = -10
if (self._speed_ramp > 150):
self._speed_ramp = 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
if (self._x_ramp < self._last_x):
self._x_ramp += 1
elif (self._x_ramp == self._last_x):
pass
else:
self._x_ramp -= 1
if (self._y_ramp < self._last_y):
self._y_ramp += 1
elif (self._y_ramp == self._last_y):
pass
else:
self._y_ramp -= 1
if (x == 0 and y == 0):
self._speed_ramp -= (self._speed_set / 10)
else:
if ((abs(self._x_ramp - x) > 20) or (abs(self._y_ramp - y) > 20)):
self._speed_ramp = 50
if (self._speed_ramp < 0):
self._speed_ramp = 0
self._last_x = x
self._last_y = y
#print (self._x_ramp, x, self._last_x, self._y_ramp, y, self._last_y, self._speed_ramp, self._speed_set)
self._robot.setMotors(self._x_ramp, self._y_ramp, self._speed_ramp)
self._robot.flushing()
self.follow()
#self.run_away()
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
self.incl = True
self.radius = 0.8
self.num_neatos = 3
self.stop_dist = 0.4
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self.goal_x = 0
self.goal_y = 0
self.center_x = 0
self.center_y = 0
# ME #
self.orient = 0
self.pose_x = 0
self.pose_y = 0
# OTHERS #
self.posea = PoseStamped().pose.position
self.poseb = PoseStamped().pose.position
self.posec = PoseStamped().pose.position
self.dista = 10
self.distb = 10
self.distc = 10
self.person_pose = 0
self.person_orient = 0
self.person_dist_cent = 10
self.person_dist_me = 10
# Include or exclude
# rospy.Subscriber("/inclusion", String, self.incl_handler, queue_size=10)
# Formation center
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
# Neatos
if chairbot_number == '01':
# self.i_am = 1
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '02':
# self.i_am = 2
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '03':
# self.i_am = 3
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '04':
# self.i_am = 4
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
# Person
rospy.Subscriber("/neato06/pose",
PoseStamped,
self.person_handler,
queue_size=10)
self.i_am = 1
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
self.incl = True
self.radius = 0.8
self.num_neatos = 3
self.stop_dist = 0.4
self._pub = rospy.Publisher("/debug", String, queue_size=10)
self.goal_x = 0
self.goal_y = 0
self.center_x = 0
self.center_y = 0
# ME #
self.orient = 0
self.pose_x = 0
self.pose_y = 0
# OTHERS #
self.posea = PoseStamped().pose.position
self.poseb = PoseStamped().pose.position
self.posec = PoseStamped().pose.position
self.dista = 10
self.distb = 10
self.distc = 10
self.person_pose = 0
self.person_orient = 0
self.person_dist_cent = 10
self.person_dist_me = 10
# Include or exclude
# rospy.Subscriber("/inclusion", String, self.incl_handler, queue_size=10)
# Formation center
rospy.Subscriber("/neato05/pose",
PoseStamped,
self.goal_handler,
queue_size=10)
# Neatos
if chairbot_number == '01':
# self.i_am = 1
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '02':
# self.i_am = 2
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '03':
# self.i_am = 3
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
if chairbot_number == '04':
# self.i_am = 4
rospy.Subscriber("/neato04/pose",
PoseStamped,
self.pose_handler,
queue_size=10)
rospy.Subscriber("/neato01/pose",
PoseStamped,
self.pose_handlera,
queue_size=10)
rospy.Subscriber("/neato02/pose",
PoseStamped,
self.pose_handlerb,
queue_size=10)
rospy.Subscriber("/neato03/pose",
PoseStamped,
self.pose_handlerc,
queue_size=10)
# Person
rospy.Subscriber("/neato06/pose",
PoseStamped,
self.person_handler,
queue_size=10)
self.i_am = 1
def fwd(self):
SPEED = 150
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(0.2)
def bck(self):
SPEED = 150
self._robot.setMotors(300, 300, SPEED)
rospy.sleep(0.2)
def turnRight(self):
SPEED = 200
self._robot.setMotors(200, -200, SPEED)
rospy.sleep(0.05)
def turnLeft(self):
SPEED = 200
self._robot.setMotors(-200, 200, SPEED)
rospy.sleep(0.05)
def stop(self):
SPEED = 00
self._robot.setMotors(00, 00, SPEED)
rospy.sleep(1)
self._robot.setMotors(00, 00, SPEED)
# def incl_handler(self,data):
# if data == 'true':
# self.incl = True
# else:
# self.incl = False
# Get goal
def goal_handler(self, data):
self.center_x = data.pose.position.x
self.center_y = data.pose.position.y
self.dist_cent = sqrt((self.center_x - self.pose_x)**2 +
(self.center_y - self.pose_y)**2)
######## I AM THIS NEATO #############
def pose_handler(self, data):
self.pose_x = data.pose.position.x
self.pose_y = data.pose.position.y
qx = data.pose.orientation.x
qy = data.pose.orientation.y
qz = data.pose.orientation.z
qw = data.pose.orientation.w
self.orient = tf.transformations.euler_from_quaternion(
[qx, qy, qz, qw])[2]
#### THESE ARE THE OTHER NEATOES ####
def pose_handlera(self, data):
self.posea = data.pose.position
self.dista = sqrt((self.posea.x - self.pose_x)**2 +
(self.posea.y - self.pose_y)**2)
def pose_handlerb(self, data):
self.poseb = data.pose.position
self.distb = sqrt((self.poseb.x - self.pose_x)**2 +
(self.poseb.y - self.pose_y)**2)
def pose_handlerc(self, data):
self.posec = data.pose.position
self.distc = sqrt((self.posec.x - self.pose_x)**2 +
(self.posec.y - self.pose_y)**2)
### THIS IS THE PERSON ######
def person_handler(self, data):
self.person_pose = data.pose.position
qx = data.pose.orientation.x
qy = data.pose.orientation.y
qz = data.pose.orientation.z
qw = data.pose.orientation.w
self.person_orient = tf.transformations.euler_from_quaternion(
[qx, qy, qz, qw])[2]
self.person_dist_cent = sqrt((self.person_pose.x - self.center_x)**2 +
(self.person_pose.y - self.center_y)**2)
# self.person_dist_me = sqrt((self.person_pose.x - self.pose_x)**2 + (self.person_pose.y -self.pose_y)**2 )
def should_stop(self):
### WHO GETS PRIORITY ###
go = 1
if self.i_am == 1:
print("I am 1 and I yield to 2, 3, and the Queen")
if self.dista <= self.stop_dist or self.distb <= self.stop_dist or self.distc <= self.stop_dist:
go = 0
elif self.i_am == 2:
print("I am 2 and I yield to 3 and the Queen")
if self.distb <= self.stop_dist or self.distc <= self.stop_dist:
go = 0
elif self.i_am == 3:
print("I am 3 and I yield to the Queen")
if self.distc <= self.stop_dist:
go = 0
elif self.i_am == 4:
print("I am the Queen")
else:
print("She doesn't even go here")
return go
def get_goal_circle(self):
self.ang_increment = 2 * 3.1415 / self.num_neatos
# if self.i_am <=self.num_neatos:
# neato = self.i_am
# else:
# neato = 0
if self.i_am == 1:
neato = 1
elif self.i_am == 3:
neato = 2
else:
neato = 0
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
self.goal_angle = atan2(self.goal_y - self.pose_y,
self.goal_x - self.pose_x)
# self._pub.publish("goal01: "+ str(neato*self.ang_increment))
def get_intermediate_goal(self):
m = -(self.pose_y - self.goal_y) / (self.pose_x - self.goal_x)
b = (self.center_y / self.center_x) - m
## If within stop dist of formation center, make an intermediate goal
## relative to line between position and goal
pass
def follow(self):
current_angle = self.orient
ang_error = self.goal_angle - current_angle
if abs(ang_error - 2 * 3.1415) <= 0.15:
ang_error = 0
pos_error = sqrt((self.goal_x - self.pose_x) *
(self.goal_x - self.pose_x) +
(self.goal_y - self.pose_y) *
(self.goal_y - self.pose_y))
### CHECK IF TOO CLOSE TO OTHER NEATOS ###
go = self.should_stop()
### ONLY MOVE IF NOT STOPPED BY OTHER NEATOS ###
if go == 1:
if ang_error > 0:
if abs(ang_error) > 0.2:
self.turnLeft()
# self._pub.publish("angle error: " +str(ang_error))
else:
if abs(ang_error) > 0.2:
self.turnRight()
# self._pub.publish("angle error: " +str(ang_error))
if abs(ang_error) <= 0.2:
if pos_error > 0.5 * self.stop_dist:
self.fwd()
# self._pub.publish("pose01 error: " +str(pos_error))
else:
self.stop()
else:
self.stop()
def person_adjust(self, incl):
person_dist04 = sqrt((self.person_pose.x - self.posec.x)**2 +
(self.person_pose.y - self.posec.y)**2)
person_dist03 = sqrt((self.person_pose.x - self.poseb.x)**2 +
(self.person_pose.y - self.poseb.y)**2)
person_dist01 = sqrt((self.person_pose.x - self.pose_x)**2 +
(self.person_pose.y - self.pose_y)**2)
# if self.i_am <=self.num_neatos:
# neato = self.i_am
# else:
# neato = 0
incl = 1
if self.i_am == 1:
neato = 1
elif self.i_am == 3:
neato = 2
else:
neato = 0
if person_dist01 < person_dist04 or person_dist01 < person_dist03:
if person_dist04 < person_dist03:
self._pub.publish("between 4 and 1" + str(incl))
# 01 or 04 move
# person between 0 and 120
# if person_dist01 < person_dist04:
# # closer to 01 so i move to 180, which is orig angle +ang_inc/2
# # robot03 (at 240) will move to 270, which is orig angle +ang_inc/4
# # robot04 (at 360) is still
if incl == 1:
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 2) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 2) + self.center_y
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 4) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 4) + self.center_y
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
else:
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_y
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_y
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
elif person_dist03 < person_dist04:
self._pub.publish("between 3 and 1" + str(incl))
# 01 or 03 move
# person between 120 and 240
# if person_dist01 < person_dist03:
# # closer to 01 (at 120) move to 60
# # robot04 (at 0/360) will move to 330
# # robot03 is still
if incl == 1:
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment -
self.ang_increment / 2) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment -
self.ang_increment / 2) + self.center_y
self._pub.publish(
str(neato * self.ang_increment -
self.ang_increment / 2))
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment -
self.ang_increment / 4) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment -
self.ang_increment / 4) + self.center_y
else:
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_y
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_y
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
else:
# between 240 and 360
# if person_dist04 < person_dist03:
#closer ro 04 so 04 moves to 60
# robot01 moves to 150
# robot03 is still
self._pub.publish("between 4 and 3" + str(incl))
if incl == 1:
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 4) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 4) + self.center_y
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 2) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 2) + self.center_y
else:
if self.i_am == 4:
self.goal_x = self.radius * cos(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment -
self.ang_increment / 3) + self.center_y
if self.i_am == 3:
self.goal_x = self.radius * cos(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment +
self.ang_increment / 3) + self.center_y
if self.i_am == 1:
self.goal_x = self.radius * cos(
neato * self.ang_increment) + self.center_x
self.goal_y = self.radius * sin(
neato * self.ang_increment) + self.center_y
self.goal_angle = atan2(self.goal_y - self.pose_y,
self.goal_x - self.pose_x)
def spin(self):
self._robot.flushing()
self.get_goal_circle()
if self.person_dist_cent <= 1.5 * self.radius:
self.person_adjust(self.incl)
# if self.dist_cent <= self.stop_dist:
# self.get_intermediate_goal()
self.follow()
#self.run_away()
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('controller' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato")
rospy.loginfo("Using port: %s" % (self._port))
self.ramp_rate = rospy.get_param("~ramp_rate", 0.3)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self._robot = Botvac(self._port)
#self.pub_motor = rospy.Publisher('roboBaseSub', NeatoCommand, queue_size=10)
self.rate = rospy.get_param("~rate", 20)
self.w = rospy.get_param("~base_width", 0.49)
self.ramping_enable = rospy.get_param("~ramping_enable", False)
self.velfactor = rospy.get_param("~mps_to_rpm", 1)
self.prev_left_vel = 0
self.prev_right_vel = 0
rospy.Subscriber('/neato01/cmd_vel_mux/input/navi', Twist,
self.twistCallback)
#############################################################
def spin(self):
#############################################################
r = rospy.Rate(self.rate)
idle = rospy.Rate(20)
then = rospy.Time.now()
self.ticks_since_target = self.timeout_ticks
###### main loop ######
while not rospy.is_shutdown():
while not rospy.is_shutdown(
): # and self.ticks_since_target < self.timeout_ticks:
self.spinOnce()
r.sleep()
idle.sleep()
#############################################################
###########################################################################
def ramp_velocity(self, target_vel, previous_vel, ramp_rate):
if (target_vel >= previous_vel):
sign = 1
else:
sign = -1
step_size = ramp_rate / self.rate
delta = fabs(target_vel - previous_vel)
if delta >= step_size:
command_vel = previous_vel + sign * step_size
else:
command_vel = target_vel
return command_vel
#############################################################
def spinOnce(self):
#############################################################
self.left = 1.0 * self.dx - self.dr * self.w / 2
self.right = 1.0 * self.dx + self.dr * self.w / 2
###RAMPING SECTION#########
if self.ramping_enable:
self.prev_left_vel = self.ramp_velocity(self.left,
self.prev_left_vel,
self.ramp_rate)
self.prev_right_vel = self.ramp_velocity(self.right,
self.prev_right_vel,
self.ramp_rate)
self.cmdVel[0] = int(self.prev_left_vel * self.velfactor)
self.cmdVel[1] = int(self.prev_right_vel * self.velfactor)
###########################
self.cmdVel[0] = int(self.left * self.velfactor)
self.cmdVel[1] = int(self.right * self.velfactor)
#############################################################
#self.pub_motor.publish(self.cmdVel)
self._robot.setMotors(self.cmdVel[0], self.cmdVel[1], 150)
self.ticks_since_target += 1
def twistCallback(self, msg):
#############################################################
# rospy.loginfo("-D- twistCallback: %s" % str(msg))
self.ticks_since_target = 0
self.dx = msg.linear.x
self.dr = msg.angular.z
self.dy = msg.linear.y
class NeatoNode:
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('teleop' + chairbot_number, anonymous=True)
self._port = rospy.get_param('~neato_port', "/dev/neato_port")
rospy.loginfo("Using port: %s" % (self._port))
self._robot = Botvac(self._port)
rospy.Subscriber("/joy" + chairbot_number,
Joy,
self.joy_handler,
queue_size=10)
rospy.Subscriber("/cbon" + chairbot_number,
Int8,
self.cbon,
queue_size=10)
rospy.Subscriber('/touches' + chairbot_number,
Int8,
self.touch_handler,
queue_size=10)
rospy.Subscriber('/touches',
Int8,
self.global_touch_handler,
queue_size=10)
self._global_touch = rospy.Publisher('/touches', Int8, queue_size=10)
self._joystick_axes = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
self._joystick_buttons = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self._speed = 0
self._distance = 20
self._speed_set = 0
self._speed_ramp = 0
self._last_x = 0
self._last_y = 0
self._x_ramp = 0
self._y_ramp = 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
# 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 = 75
self._robot.setMotors(-100, -100, SPEED)
rospy.sleep(1)
def fwd(self):
SPEED = 100
self._robot.setMotors(-300, -300, SPEED)
rospy.sleep(1)
def back(self):
SPEED = 200
self._robot.setMotors(200, 200, SPEED)
rospy.sleep(1)
def right(self):
SPEED = 150
self._robot.setMotors(150, -150, SPEED)
rospy.sleep(1)
def left(self):
SPEED = 150
self._robot.setMotors(-150, 150, 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._joystick_buttons = ps.buttons
self._joystick_axes = ps.axes
def cbon(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 global_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 == 7:
self._touch7 = not self._touch7
self._touch6 = self._touch7
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:
pass
# self._touch7 = False
# self._touch6 = False
if self._touch_number == 20:
self._touch8 = False
def touch_handler(self, msg):
self._touch_number = msg.data
if self._touch_number == 6:
self._touch6 = not self._touch6
if self._touch6 or self._touch_number == 7:
self._global_touch.publish(self._touch_number)
def spin(self):
Lft_t = self._joystick_axes[0]
Lft_d = self._joystick_axes[1]
Rgh_t = self._joystick_axes[3]
Rgh_d = self._joystick_axes[4]
AageL = self._joystick_axes[2]
AageR = self._joystick_axes[5]
L_R = self._joystick_axes[6]
F_B = self._joystick_axes[7]
sq = self._joystick_buttons[0]
xx = self._joystick_buttons[1]
ci = self._joystick_buttons[2]
tr = self._joystick_buttons[3]
self._speed_s = self._joystick_buttons[4]
self._speed_f = self._joystick_buttons[5]
AageL_Button = self._joystick_buttons[6]
AageR_Button = self._joystick_buttons[7]
share = self._joystick_buttons[8]
options = self._joystick_buttons[9]
pressL = self._joystick_buttons[10]
pressR = self._joystick_buttons[11]
power = self._joystick_buttons[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._speed_set = self._speed
ll = (Lft_d * self._distance)
rr = (Rgh_t * self._distance)
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._speed_ramp - self._speed_set)
if (self._speed_ramp < self._speed_set):
self._speed_ramp += (speeddif / 20)
else:
self._speed_ramp -= (speeddif / 20)
if (self._speed_ramp < 0):
self._speed_ramp = 0
elif (self._speed_ramp > 330):
self._speed_ramp = 330
if (self._speed_set > 150):
if (0 < x < 10):
x = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < x < 0):
x = -10
if (self._speed_ramp > 150):
self._speed_ramp = 150
if (0 < y < 10):
y = 10
if (self._speed_ramp > 150):
self._speed_ramp = 150
elif (-10 < y < 0):
y = -10
if (self._speed_ramp > 150):
self._speed_ramp = 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
if (self._x_ramp < self._last_x):
self._x_ramp += 1
elif (self._x_ramp == self._last_x):
pass
else:
self._x_ramp -= 1
if (self._y_ramp < self._last_y):
self._y_ramp += 1
elif (self._y_ramp == self._last_y):
pass
else:
self._y_ramp -= 1
if (x == 0 and y == 0):
self._speed_ramp -= (self._speed_set / 10)
else:
if ((abs(self._x_ramp - x) > 20) or (abs(self._y_ramp - y) > 20)):
self._speed_ramp = 50
if (self._speed_ramp < 0):
self._speed_ramp = 0
self._last_x = x
self._last_y = y
print(self._x_ramp, x, self._last_x, self._y_ramp, y, self._last_y,
self._speed_ramp, self._speed_set)
self._robot.setMotors(self._x_ramp, self._y_ramp, self._speed_ramp)
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._touch2 or self._touch3):
self.right()
if (self._touch4):
self.back()
if (self._touch5):
self.fwd()
else:
pub_led.publish(0)
def shutdown(self):
self._robot.setLDS("off")
self._robot.setTestMode("off")