def __init__(self):
self.key = None # Assign key to something (it doesnt really matter what)
# Initialize ROS publisher
self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node('teleop_twist') # Initialize ros node
self.subBump = rospy.Subscriber('/bump',Bump, bump_Callback)
self.subLaser = rospy.Subscriber('/stable_scan', LaserScan, laser_Callback)
rospy.spin()
move = rospy.get_param("~moving", .5)
turn = rospy.get_param("~turn", 1.0)
self.bumpData = Bump(0,0,0,0)
self.laserData = None
def bump_Callback(self, msg):
#Callback for bump sensors
self.bumpData = msg
def laser_Callback(self, msg):
#callback for laser data
self.laserData = msg.ranges
def has_bumped(self):
# Takes no args, returns boolean based on bump or not
if self.bumpData.leftFront or self.bumpData.rightFront or self.bumpData.leftSide or self.bumpData.rightSide:
return True
return False
def detect_obstacle(self):
# Takes no args, returns boolean based on object
for data in self.laserData:
if 0 < data < 0.5:
return True
return False
def runRobot(self):
while self.key != '\x03': # some fanciness from Paul to use Ctrl-C
key = getKey()
if key in moveBindings.keys():
move = moveBindings[key]
elif key in turnBindings.keys():
turn = turnBindings[key]
else:
move = 0
turn = 0
twist = Twist(linear=Vector3(y=0,z=0), angular=Vector3(x=0,y=0))
twist.linear.x = move
twist.angular.z = turn
if key == stopKey:
twist.linear.x = 0
twist.angular.z = 0
elif self.has_bumped():
self.state = self.pub.publish(Twist(Vector3(-0.15,0,0),Vector3(0,0,0)))
elif self.detect_obstacle():
self.pub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0.25)))
else:
self.pub.publish(twist) #run these jewels fast
def __init__(self):
self.countby5 = 0
self.roomba_speed = 0.10
self.roomba_turn_speed = -1.0
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.last_bump = Bump()
rospy.Subscriber('bump',Bump,self.callback)
rospy.init_node('RoombaCode')
rospy.Timer(rospy.Duration(5.),self.Turn)
self.practice()
self.tfFrames = tf.TransformListener()
rospy.on_shutdown(self.stop)
def spin(self):
encoders = [0, 0]
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 = 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_link')
# main loop of driver
r = rospy.Rate(5)
rospy.sleep(4)
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
left, right = self.robot.getMotors()
# now update position information
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
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)
total_dth += dth
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 = curr_motor_time
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
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
curr_motor_time, "base_link", "odom")
self.odomPub.publish(odom)
print 'Got motors %f' % (time.time() - t_start)
except:
pass
t_start = time.time()
bump_sensors = self.robot.getDigitalSensors()
# 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])))
print 'Set motors %f' % (time.time() - t_start)
# publish everything
self.scanPub.publish(scan)
self.bumpPub.publish(
Bump(leftFront=bump_sensors[0],
leftSide=bump_sensors[1],
rightFront=bump_sensors[2],
rightSide=bump_sensors[3]))
self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
# wait, then do it again
r.sleep()
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# NEED to reorder the laser scans and flip the laser around... this will not be intuitive for students!!
# 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 = -pi
scan.angle_max = pi
scan.angle_increment = pi / 180.0
scan.range_min = 0.020
scan.range_max = 5.0
scan.time_increment = 1.0 / (5 * 360)
scan.scan_time = 0.2
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
iter_count = 0
rospy.sleep(4)
#self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
#print "spinning"
iter_count += 1
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
self.robot.requestScan()
#time.sleep(.01)
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
if abs(delta_t - 0.2) > 0.1:
print "unexpected getldsscan ", delta_t
scan.header.stamp = rospy.Time.now()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
# repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
scan.ranges.append(scan.ranges[0])
scan.intensities.append(scan.intensities[0])
#print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
start_t = rospy.Time.now()
left, right, left_speed, right_speed = self.robot.getMotors()
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
if delta_t > 0.1:
print "unexpected get_motors ", delta_t
#print left, right
# now update position information
# might consider moving curr_motor_time down
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
# might consider using speed instead!
#print d_left, d_right, left_speed*dt/1000.0, right_speed*dt/1000.0
#d_left, d_right = left_speed*dt/1000.0, right_speed*dt/1000.0
encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
total_dth += dth
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 = curr_motor_time
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
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
curr_motor_time, "base_link", "odom")
self.odomPub.publish(odom)
#print 'Got motors %f' % (time.time() - t_start)
except Exception as err:
print "my error is " + str(err)
t_start = time.time()
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
try:
bump_sensors = self.robot.getDigitalSensors()
self.bumpPub.publish(
Bump(leftFront=bump_sensors[0],
leftSide=bump_sensors[1],
rightFront=bump_sensors[2],
rightSide=bump_sensors[3]))
except:
print "failed to get bump sensors!"
# # send updated movement commands
#print 'Set motors %f' % (time.time() - t_start)
# publish everything
#print "publishing scan!"
self.scanPub.publish(scan)
# wait, then do it again
r.sleep()
def spin(self):
old_ranges = None
encoders = [0, 0]
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 = -pi
scan.angle_max = pi
scan.angle_increment = pi / 180.0
scan.range_min = 0.020
scan.range_max = 5.0
scan.time_increment = 1.0 / (5 * 360)
scan.scan_time = 0.2
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(20)
rospy.sleep(4)
# do UDP hole punching to make sure the sensor data from the robot makes it through
self.robot.do_udp_hole_punch()
self.robot.send_keep_alive()
last_keep_alive = time.time()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
last_set_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
if time.time() - last_keep_alive > 30.0:
self.robot.send_keep_alive()
last_keep_alive = time.time()
self.robot.requestScan()
new_stamp = rospy.Time.now()
delta_t = (new_stamp - scan.header.stamp).to_sec()
scan.header.stamp = new_stamp
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
# repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
# This is important in order to adhere to ROS conventions regarding laser scanners
if len(scan.ranges):
scan.ranges.append(scan.ranges[0])
scan.intensities.append(scan.intensities[0])
if old_ranges == scan.ranges:
scan.ranges, scan.intensities = [], []
else:
old_ranges = copy(scan.ranges)
if delta_t - 0.2 > 0.1:
print "Iteration took longer than expected (should be 0.2) ", delta_t
# get motor encoder values
curr_motor_time = rospy.Time.now()
try:
motors = self.robot.getMotors()
if motors:
# unpack the motor values since we got them.
left, right = motors
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
# now update position information
# might consider moving curr_motor_time down
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
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)
total_dth += dth
x_init = self.x
y_init = self.y
th_init = self.th
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
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
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.pose.covariance = [
10**-1, 0, 0, 0, 0, 0, 0, 10**-1, 0, 0, 0, 0, 0, 0,
10**-1, 0, 0, 0, 0, 0, 0, 10**5, 0, 0, 0, 0, 0, 0,
10**5, 0, 0, 0, 0, 0, 0, 10**5
]
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0), (quaternion.x, quaternion.y,
quaternion.z, quaternion.w),
curr_motor_time, "base_link", "odom")
self.odomPub.publish(odom)
except Exception as err:
print "my error is " + str(err)
with self.cmd_vel_lock:
if self.cmd_vel:
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
self.cmd_vel = None
elif rospy.Time.now() - last_set_motor_time > rospy.Duration(
.2):
self.robot.resend_last_motor_command()
last_set_motor_time = rospy.Time.now()
try:
bump_sensors = self.robot.getDigitalSensors()
if bump_sensors:
self.bumpPub.publish(
Bump(leftFront=bump_sensors[0],
leftSide=bump_sensors[1],
rightFront=bump_sensors[2],
rightSide=bump_sensors[3]))
except:
print "failed to get bump sensors!"
try:
accelerometer = self.robot.getAccel()
if accelerometer:
self.accelPub.publish(
Accel(accelXInG=accelerometer[2],
accelYInG=accelerometer[3],
accelZInG=accelerometer[4]))
except Exception as err:
print "failed to get accelerometer!", err
if len(scan.ranges):
self.scanPub.publish(scan)
# wait, then do it again
r.sleep()