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.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('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.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('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.distPub = rospy.Publisher('dist', 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()
# 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')
dist = Vector3Stamped(header=rospy.Header(frame_id="odom"))
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
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
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)
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, "base_footprint", "odom")
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 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)]
