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