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.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('soft_button', Button, queue_size=100) 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 = -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') softb = Button() # main loop of driver r = rospy.Rate(5) while not rospy.is_shutdown(): # get motor encoder values left, right = self.robot.getMotors() # send updated movement commands if self.cmd_vel != self.old_vel: self.robot.setMotors( self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1]))) # prepare laser scan scan.header.stamp = rospy.Time.now() 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] 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 # 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( ) softb.value = btn_soft softb.name = "Soft_Button" # 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) self.buttonPub.publish(softb) # 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 * (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.port = rospy.get_param('~port', "/dev/ttyUSB0") self.lds = rospy.get_param('~lds', True) rospy.loginfo("Using port: %s" % self.port) self.robot = Botvac(self.port, self.lds) rospy.Subscriber("cmd_dist", Movement, self.cmdMovementCb) self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=1) self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1) self.buttonPub = rospy.Publisher('button', Button, queue_size=1) self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=1) self.accelerationPub = rospy.Publisher('acceleration', Vector3Stamped, queue_size=1) self.wallPub = rospy.Publisher('wall', Range, queue_size=1) self.drop_leftPub = rospy.Publisher('drop_left', Range, queue_size=1) self.drop_rightPub = rospy.Publisher('drop_right', Range, queue_size=1) self.magneticPub = rospy.Publisher('magnetic', Sensor, queue_size=1) self.odomBroadcaster = TransformBroadcaster() rospy.Service('set_info_led', SetLed, self.setInfoLed) rospy.Service('play_sound', PlaySound, self.playSound) rospy.Service('set_lds', SetBool, self.setLDS) self.cmd_vel = 0 self.cmd_dist = [0, 0] self.old_dist = self.cmd_dist self.encoders = [0, 0] 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 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)] def cmdMovementCb(self, req): k = req.vel # sending commands higher than max speed will fail if k > self.robot.max_speed: k = self.robot.max_speed ros.logwarn( "You have set the speed to more than the maximum speed of the neato. For safety reasons it is set to %d", self.robot.max_speed) self.robot.cmd_vel = k self.robot.cmd_dist = [req.x_dist, req.y_dist] def publish_odom(self, odom): # get motor encoder values left, right = self.robot.getMotors() d_left = (left - self.encoders[0]) / 1000.0 d_right = (right - self.encoders[1]) / 1000.0 self.encoders = [left, right] 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 # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) dt = (odom.header.stamp - rospy.Time.now()).secs # 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.odomPub.publish(odom) self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), "base_link", "odom") def publish_scan(self, scan): scan.header.stamp = rospy.Time.now() scan.ranges = self.robot.getScanRanges() self.scanPub.publish(scan) def publish_analog(self, acceleration, range_sensor, magnetic): # analog sensors ax, ay, az, ml, mr, wall, drop_left, drop_right = self.robot.getAnalogSensors( ) acceleration.header.stamp = rospy.Time.now() # convert mG to m/s^2 acceleration.vector.x = ax * 9.80665 / 1000.0 acceleration.vector.y = ay * 9.80665 / 1000.0 acceleration.vector.z = az * 9.80665 / 1000.0 range_sensor.header.stamp = rospy.Time.now() self.accelerationPub.publish(acceleration) range_sensor.range = wall / 1000.0 self.wallPub.publish(range_sensor) range_sensor.range = drop_left / 1000.0 self.drop_leftPub.publish(range_sensor) range_sensor.range = drop_right / 1000.0 self.drop_rightPub.publish(range_sensor) magnetic_enum = ("Left_Sensor", "Right_Sensor") for idx, val in enumerate((ml, mr)): magnetic.value = val magnetic.name = magnetic_enum[idx] self.magneticPub.publish(magnetic) return drop_left, drop_right, ml, mr def publish_buttons(self, button): btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons( ) button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button") 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) def publish_digital(self, sensor): lsb, rsb, lfb, rfb, lw, rw = self.robot.getDigitalSensors() sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper", "Left_Wheel", "Right_Wheel") for idx, b in enumerate((lsb, rsb, lfb, rfb, lw, rw)): if b == 1: sensor.value = b sensor.name = sensor_enum[idx] self.sensorPub.publish(sensor) return lw, rw, lsb, rsb, lfb, rfb def set_battery_status(self): # notify if low batt charge = self.robot.getCharger() if charge < 10: #print "battery low " + str(self.robot.getCharger()) + "%" self.robot.setLED("battery", "red", "pulse") elif charge < 25: self.robot.setLED("battery", "yellow", "solid") else: self.robot.setLED("battery", "green", "solid") rospy.loginfo_throttle(60, "Current battery status: " + str(charge) + "%") def violate_safety_constraints(left_drop, right_drop, *digital_sensors): if left_drop > 30 or right_drop > 30: print "safety constraint violated by drop sensor" return True else: for sensor in digital_sensors: if sensor == 1: print "safety constraint violated by digital sensor" return True return False def setInfoLed(self, data): self.robot.setLED("info", data.color, data.status) return SetLedResponse() def playSound(self, data): self.robot.playSound(data.soundid) return PlaySoundResponse() def setLDS(self, data): if data.data: self.robot.setLDS("on") else: self.robot.setLDS("off") return SetBoolResponse(True, "")
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): 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) self.robot.requestScan() 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(): # start lidar data self.robot.getScanRanges() lidarData = self.robot.requestScan() print(lidarData) # TODO stop if something is in front # end lidar data 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/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)]
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 self.obstacle = False # used by collision avoidance def spin(self): # main loop of driver r = rospy.Rate(20) DIRECTION = 1 # 1 for forward, -1 for backwards self.robot.requestScan() # FIXME time.sleep(1) while not rospy.is_shutdown(): self.robot.requestScan() # FIXME scan = self.robot.getScanRanges() # print(type(scan), scan) if len(scan) > 100: self.obstacle = classifyLidar(scan) # collision avoidance print('GOT NEW LIDAR DATA!!!', self.obstacle) z = self.latest_twist.angular.z x = self.latest_twist.linear.x if self.obstacle: # print('Obstacle detected, not moving!!!!!!!') SPEED = 0 dist_l = 0 dist_r = 0 x = 0 else: print('No obstacles, continuing motion ------') 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)