本文整理汇总了Python中Robot.Robot.pose方法的典型用法代码示例。如果您正苦于以下问题：Python Robot.pose方法的具体用法？Python Robot.pose怎么用？Python Robot.pose使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Robot.Robot的用法示例。
在下文中一共展示了Robot.pose方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: testMotion
# 需要导入模块: from Robot import Robot [as 别名]
# 或者: from Robot.Robot import pose [as 别名]
def testMotion(self):
global helpList, helpTheta
sample = RobotMessage()
sample.leftMotorTacho = 0
sample.rightMotorTacho = 0
sample.leftSensor = [0]
sample.frontSensor = [43,0]
sample.timeStamp = 0
robot = Robot()
pose = Pose(20, 0, 90*math.pi/180)
robot.pose = pose
assert robot.sensorWheelAngle == 90*math.pi/180
robot.motion(sample)
assert robot.poseList[0] == pose
robot.updateMap(sample, helpMapCallBack)
# move forward
moveDistance = round(360/360*math.pi*robot.wheelDiameter) #14
sample.leftMotorTacho = 360
sample.rightMotorTacho = 360
sample.leftSensor = [12.3, 12.3, 12.3]
sample.frontSensor = [43-moveDistance,0]
sample.timeStamp = 0
robot.motion(sample)
pose = Pose(20, 14, 90*math.pi/180)
assert robot.pose == pose
sx, sy, lx, ly, rx, ry = 20, 14+7, round(20-7.7), 14, round(20+7.7), 14
assert (sx, sy, lx, ly, rx, ry) == robot.getRobotShape(pose)
robot.updateMap(sample, helpMapCallBack)
testList = [(0, round(14/3)), (0, round(14/3*2)), (0, round(14/3*3)), (20, 50)]
assert testList == helpList
assert helpTheta[0] == 90*math.pi/180
# turn right 45 degree
# deltatacho = 45/180*math.pi*robot.TurnRadius)/(robot.wheelDiameter*math.pi)*360
helpList = []
helpTheta = []
sample.leftMotorTacho = 360 + round((45*robot.TurnRadius)/(robot.wheelDiameter)*2)
sample.rightMotorTacho = 360 - round((45*robot.TurnRadius)/(robot.wheelDiameter)*2)
sample.leftSensor = [12.3, 12.3, 12.3]
sample.frontSensor = [43,0]
sample.timeStamp = 0
robot.motion(sample)
pose = Pose(20, 14, round((90-45)*math.pi/180, 2))
robot.pose.theta = round(robot.pose.theta, 2)
assert robot.pose == pose
moveWheel = round(7.7*math.cos(45/180*math.pi))
moveSensor = round(7.0*math.cos(45/180*math.pi))
pose = Pose(20, 14, (90-45)*math.pi/180)
sx, sy, lx, ly, rx, ry = 20+moveSensor, 14 + moveSensor, 20 - moveWheel, 14 + moveWheel, 20 + moveWheel, 14 - moveWheel
assert (sx, sy, lx, ly, rx, ry) == robot.getRobotShape(pose)
testList = []
robot.updateMap(sample, helpMapCallBack)
assert testList == helpList
#keep move forward with this angle
helpList = []
sample.leftMotorTacho = 360 + round((45*robot.TurnRadius)/(robot.wheelDiameter)*2) + 360
sample.rightMotorTacho = 360 - round((45*robot.TurnRadius)/(robot.wheelDiameter)*2) + 360
sample.leftSensor = [20, 20, 20]
sample.frontSensor = [15]
sample.timeStamp = 0
robot.motion(sample)
pose = Pose(20 + round(14*math.cos(45/180*math.pi)), 14+ round(14*math.cos(45/180*math.pi)), round((90-45)*math.pi/180, 2))
robot.pose.theta = round(robot.pose.theta, 2)
assert robot.pose == pose
# robot moved 14 again, each distance is 4.6, then, deltaX = sqrt(4.6*4.6/2), the sample point to sensor is 20
# then, the delta(sensorX, samplepointX) is sqrt(20*20/2), the sampleX = lx + sqrt(4.6*4.6/2) - sqrt(20*20/2)
# sampleY = ly + sqrt(4.6*4.6/2) + sqrt(20*20/2)
testList = [(4, 36), (8, 40), (11, 43), (46, 40)]
robot.updateMap(sample, helpMapCallBack)
assert testList == helpList
#Robot turn left 180
sample.leftMotorTacho = 360 + round((45*robot.TurnRadius)/(robot.wheelDiameter)*2) + 360 - round((180*robot.TurnRadius)/(robot.wheelDiameter)*2)
sample.rightMotorTacho = 360 - round((45*robot.TurnRadius)/(robot.wheelDiameter)*2) + 360 + round((180*robot.TurnRadius)/(robot.wheelDiameter)*2)
sample.leftSensor = [20, 20, 20]
sample.frontSensor = [15]
sample.timeStamp = 0
robot.motion(sample)
pose = Pose(pose.x, pose.y, pose.theta + math.pi)
assert pose.x == robot.pose.x and pose.y == robot.pose.y and round(pose.theta, 2) == round(robot.pose.theta, 2)
#Robot move 360 count
helpList = []
#.........这里部分代码省略.........
示例2: Robot
# 需要导入模块: from Robot import Robot [as 别名]
# 或者: from Robot.Robot import pose [as 别名]
screen = pygame.display.set_mode(size)
screen.fill((255,255,255))
if __name__ == '__main__':
# frame delay ms
delay = 20
width = 600
height = 600
ev3Ip = "192.168.0.16"
ev3Port = 5000
robot = Robot()
robot.pose = Pose(500, 100, 90*math.pi/180)
robot.poseList.append(robot.pose)
map = GridMap(600, 600, 1)
robotConn = RobotConnection()
robotConn.remoteIp = ev3Ip
robotConn.remotePort = ev3Port
robotConn.rawDataFileName = "rawdata/data_map3"
robotConn.connectToEv3(simulate=True)
message = RobotMessage()
pygame.init()
pygame.display.set_caption('Ev3 Map')
size = [width, height]
screen = pygame.display.set_mode(size)
screen.fill((255,255,255))