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(5)
cmd_rate= self.CMD_RATE
self.robot.requestScan()
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()
#scan.ranges, scan.intensities = self.robot.getScanRanges()
scan.ranges = self.robot.getScanRanges()
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]
#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 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 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 __init__(self):
self.default_frame_id = '/base_laser_link'
self.sequence = 1
self.pub = rospy.Publisher('scan', LaserScan, queue_size=1)
self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
sensor = Sensor()
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
#rospy.loginfo("spin: before LaserScan")
# 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
#rospy.loginfo("spin: before Odometry")
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')
button = Button()
sensor = Sensor()
# main loop of driver
r = rospy.Rate(2)
cmd_rate= self.CMD_RATE
rospy.loginfo(">>> spin: before while loop <<<")
while not rospy.is_shutdown():
left, right = self.robot.getMotors()
# prepare laser scan
scan.header.stamp = rospy.Time.now()
self.robot.requestScan()
#rospy.loginfo("spin: loop: requestScan")
scan.ranges = self.robot.getLdsScan()
#if len(scan.ranges) == 0:
# scan.ranges = self.robot.getLdsScan()
# 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()
#rospy.loginfo("spin: loop: 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")
#rospy.loginfo("spin: loop: sendTransform")
if len(scan.ranges) > 0:
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
#rospy.loginfo("spin: loop: before sleep()")
r.sleep()
# Steve: testing with longer sleep
#time.sleep(1)
# shut down
rospy.loginfo(">>>>>> Exiting <<<<<<<<")
def __init__(self):
self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10)
sensor = Sensor()