コード例 #1
0
ファイル: discrete_full.py プロジェクト: rmeertens/ROSMAV
    def __init__(self):
        print("init")
        port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/rfcomm0")
        self.create = Create(port)

        self.bump = False
        self.hz = 30
        self.rate = 1. / self.hz
        self.speed = 200
        self.turn = 50
        self.distance = 85
        self.threshold = .5
        self.angle = 18
        self.lock = Lock()
        self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)

        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        self.then = datetime.now()
        self.x = 0
        self.y = 0
        self.th = 0
        self.fields = [
            'wheeldropCaster', 'wheeldropLeft', 'wheeldropRight', 'bumpLeft',
            'bumpRight', 'wall', 'cliffLeft', 'cliffFronLeft',
            'cliffFrontRight', 'cliffRight', 'virtualWall', 'infraredByte',
            'advance', 'play', 'distance', 'angle', 'chargingState', 'voltage',
            'current', 'batteryTemperature', 'batteryCharge',
            'batteryCapacity', 'wallSignal', 'cliffLeftSignal',
            'cliffFrontLeftSignal', 'cliffFrontRightSignal',
            'cliffRightSignal', 'homeBase', 'internalCharger', 'songNumber',
            'songPlaying'
        ]

        self.create.update = self.sense
コード例 #2
0
	def __init__(self):
		port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher('sensorPacket', SensorPacket)
		self.odomPub = rospy.Publisher('odom',Odometry)
		self.odomBroadcaster = TransformBroadcaster()
		self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
		self.then = datetime.now() 
		self.x = 0
		self.y = 0
		self.th = 0
		#self.create.update = self.sense
		self.create.update = None
コード例 #3
0
	def __init__(self):
		# If /create/use_host_name is True, the topic names used by
		# this driver will be /hostname/cmd_vel, /hostname/odom, etc.
		self.hn = HName(usename = rospy.get_param('/create/use_host_name', False))
		port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
		self.create = Create(port)
		self.packetPub = rospy.Publisher(self.hn.topic('sensorPacket'), SensorPacket)
		self.odomPub = rospy.Publisher(self.hn.topic('odom'),Odometry)
		self.odomBroadcaster = TransformBroadcaster()
		self.fields = ['wheeldropCaster','wheeldropLeft','wheeldropRight','bumpLeft','bumpRight','wall','cliffLeft','cliffFronLeft','cliffFrontRight','cliffRight','virtualWall','infraredByte','advance','play','distance','angle','chargingState','voltage','current','batteryTemperature','batteryCharge','batteryCapacity','wallSignal','cliffLeftSignal','cliffFrontLeftSignal','cliffFrontRightSignal','cliffRightSignal','homeBase','internalCharger','songNumber','songPlaying']
		self.then = datetime.now()
		self.x = 0
		self.y = 0
		self.th = 0
		self.idealTh = 0
		self.create.update = self.sense

		# SW
		self.create.storeSong(2,67,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,69,11,79,32,74,32,72,11,71,11,72,11,69,32)
		# CE3K
		self.create.storeSong(3,74,16,81,24,83,24,79,24,67,32,74,40)
		# HN
		self.create.storeSong(4,74,64,78,20,75,20,74,20,78,56,81,17,79,17,78,17,79,48,82,14,81,14,79,14,78,28,75,32,74,40)
		# Js
		self.create.storeSong(5,42,32,43,32,42,32,43,32,42,32,43,32,42,32,43,32)
		# ROLA
		self.create.storeSong(6,64,24,65,8,67,16,72,40,62,24,64,8,65,48,67,24,69,8,71,16,77,40,69,24,71,10,72,24,74,24,76,48)
		# JJMD
		self.create.storeSong(7,79,12,81,12,83,12,86,12,84,12,84,12,88,12,86,12,86,12,91,12,90,12,91,12,86,12,83,12,79,40)
		# SaaHC
		self.create.storeSong(8,84,25,79,18,79,12,81,25,79,40,83,28,84,12)
		# Angry men
		self.create.storeSong(9,64,24,62,12,60,24,62,12,64,24,65,12,67,36,64,12,62,12,60,12,59,24,57,12,59,24,60,12,55,48)
		# IGR
		self.create.storeSong(10,77,24,79,24,82,24,84,40)
#,84,24,82,24,79,24,77,40,77,24,79,24,82,24,84,18,87,18,84,10,86,32,84,42)

		self.create.storeSong(11,50,10,51,10)
		self.create.storeSong(12,51,10,50,10)
		self.create.storeSong(13,52,10,51,10,50,10)
		self.create.storeSong(14,53,10,54,10,55,10)
		self.create.storeSong(15,54,10,54,10)
コード例 #4
0
ファイル: ges.py プロジェクト: rmeertens/ROSMAV

def cos(model, sample):
    a = 0.0
    b = 0.0
    c = 0.0
    for i in xrange(0, 30):
        for j in xrange(0, 3):
            a = a + model[i][j] * sample[i][j]
            b = b + model[i][j] * model[i][j]
            c = c + sample[i][j] * sample[i][j]
    return 1.0 - (a / (sqrt(b) * sqrt(c)))


#create = Create("/dev/rfcomm0")
create = Create()

a = cwiid.Wiimote()
allLEDS = cwiid.LED1_ON ^ cwiid.LED2_ON ^ cwiid.LED3_ON ^ cwiid.LED4_ON
a.led = allLEDS

a.rpt_mode = cwiid.RPT_ACC ^ cwiid.RPT_BTN

cal = a.get_acc_cal(0)
zero = cal[0]
one = cal[1]

print zero, one

create.start()
コード例 #5
0
ファイル: discrete.py プロジェクト: rmeertens/ROSMAV
    global bump, rate, lock, speed, turn, create, distance, angle, norms, threshold
    bump = False
    hz = 30
    rate = 1. / hz
    speed = 200
    turn = 50
    distance = 85
    threshold = .5
    angle = 18
    lock = Lock()

    rospy.init_node('discreteController')

    port = rospy.get_param('/brown/irobot_create_2_1/port', "/dev/ttyUSB0")
    print(port)
    create = Create(port)
    service = rospy.Service('act', Act, act)

    try:
        sleep(1)
        create.start()
        sleep(1)
        norms = [
            create.cliffLeftSignal, create.cliffFrontLeftSignal,
            create.cliffFrontRightSignal, create.cliffRightSignal
        ]
        print "\nrunning...\n"
        rospy.spin()
    except:
        pass
    finally: