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)]