def testNeedMouvement(self):
"""
verifie la methode testNeedMouvement
"""
robot1 = Robot(1, 1, "robot1")
robot2 = Robot(2, 2, "robot2")
robot3 = Robot(2, 5, "robot3")
robot4 = Robot(2, 1, "robot4")
# renvoie vrai si aucun mouvement est set
self.assertTrue(robot1.needMovement(self.map))
self.assertTrue(robot2.needMovement(self.map))
self.assertTrue(robot3.needMovement(self.map))
self.assertTrue(robot4.needMovement(self.map))
robot1.setMovement("n", "n", 1)
robot2.setMovement("e", "e", 1)
robot3.setMovement("e", "e", 1)
robot4.setMovement("m", "s", 1)
# renvoie vrai si le mouvement est impossible
self.assertTrue(robot1.needMovement(self.map))
self.assertTrue(robot4.needMovement(self.map))
# sinon false
self.assertFalse(robot2.needMovement(self.map))
self.assertFalse(robot3.needMovement(self.map))
pass
def plot_motion_model_distribution():
motion_cmd = [
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi),
MotionCommand(1, 0.25 * pi)
]
ep = ErrorParamsMovement(0.0, 0.0, 0.0, 0.0, 0.05, 0.05)
# Plot paths of 100 noisy robots
for _ in range(100):
robot_noise = Robot(0, 0, 0, False, error_params=ep, color='C4')
for command in motion_cmd:
robot_noise.move(command.v, command.omega)
robot_noise.plot()
# Plot motion of ideal motion model
robot = Robot(0, 0, 0, False, color='C0')
for command in motion_cmd:
robot.move(command.v, command.omega)
robot.plot()
plt.show(block=True)
def create_fleet(self):
robot_one = Robot('Tim', 5, 25)
robot_two = Robot('Phil', 25, 60)
robot_three = Robot('Rodney', 15, 40)
self.fleet_of_robots.append(robot_one)
self.fleet_of_robots.append(robot_two)
self.fleet_of_robots.append(robot_three)
def start(self):
last_update = rospy.Time.now().to_sec()
self.robots["robot0"] = Robot("robot0", [0, 1, 0], 4000, [4, 0])
self.robots["robot1"] = Robot("robot1", [0, 0, 1], 4000, [-4, 0])
R = rospy.Rate(150)
rospy.sleep(5)
print ("HOLD WASD to move, E to exit")
while not (rospy.is_shutdown() or self.shutdown):
delta_time = rospy.Time.now().to_sec() - last_update
for k, robot1 in self.robots.items():
if robot1.alive:
robot1(delta_time)
for k2, robot2 in self.robots.items():
try:
if k != k2:
position, quaternion = self.listener.lookupTransform("/"+robot1.damage.id, "/"+robot2.id, rospy.Time())
distance = math.sqrt(position[0] * position[0] + position[1] * position[1])
if distance < robot1.damage.length[0] / 2.0:
robot2.destroy()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
R.sleep()
last_update = rospy.Time.now().to_sec()
def test_raise_value_error(self):
# it should pass only if the result is raising a ValueError
world_size = (5, 4)
lost_robots = []
with self.assertRaises(ValueError):
wrong_x_robot = Robot(world_size, lost_robots, 55, 20, 'N')
wrong_y_robot = Robot(world_size, lost_robots, 20, 55, 'E')
wrong_direction_robot = Robot(world_size, lost_robots, 20, 20, 'K')
def __init__(self, omap_path, sparki):
self.omap = ObstacleMap(omap_path, noise_range=1)
self.rangefinder = Rangefinder(cone_width=deg2rad(15.),
obstacle_width=1.)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# create particle filter
alpha = [0.5, 0.5, 0.5, 0.5]
self.particle_filter = ParticleFilter(num_particles=50,
alpha=alpha,
robot=self.robot,
omap=self.omap)
# set message callback
self.sparki.message_callback = self.message_received
# last timestamp received
self.last_timestamp = 0
def compiling_txt(file_name):
file = open(file_name, 'rt', encoding='utf-8')
l = file.readlines()
for i in range(len(l)):
if l[i][-1] == '\n':
l[i] = l[i][:len(l[i]) - 1]
operator = l[0]
f = False
for i in range(1, len(l)):
t = formatted_command(l[i])
if (t in op_dict[operator].command_list) or ('нц' in t) or (
'пока' in t) or ('кц' in t):
f = True
else:
f = False
raise MySyntaxError('Синтаксическая ошибка в строке {}'.format(i +
1))
if f:
op = l[0]
if op == 'Чертежник':
op = Blueprinter()
elif op == 'Черепаха':
op = Turtle()
elif op == 'Робот':
op = Robot()
elif op == 'Вычислитель':
op = Calculator()
core_alg(l[1:], op)
if isinstance(op, Turtle) or isinstance(
op, Blueprinter) or op == (op, Robot):
canvas.pack()
main.mainloop()
def __init__(self, graphic=True, nb_robot=100, models=[]):
if graphic:
self.physicsClient = p.connect(p.GUI)
else:
self.physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
self.robots = []
# assignation des positions des robots aleatoirement
start_poses = np.random.randn(len(models), 3) * 4
# on fixe la hauteur a 0.3
start_poses[:, 2] = 0.3
start_orientation = p.getQuaternionFromEuler([0, 0, 0])
# on charge les models dans les robots
for i in range(len(models)):
self.robots.append(
Robot(start_poses[i], start_orientation, models[i]))
self.nb_robot = nb_robot
self.load_robots()
self.load_plane()
self.load_collision()
def main():
table = Table(5, 5)
robot = Robot(table)
interpreter = CmdInt(robot)
while (True):
interpreter.execute(input("Enter command!\n"))
def setupThree(self):
self.robots.append(Robot((500, 1000), 90))
self.hives.append(Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 - Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 3 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 5 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 7 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 9 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 11 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 13 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(
Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 + 15 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
def newRobots(x,y):
robot = Robot()
robot.set_x(x)
robot.set_y(y)
robot.set_speedX(random.choice((-1, 1)))
robot.set_speedY(random.choice((-1, 1)))
robots.add_robot(robot)
def make_default_robot(robot_name, path_color):
start_point = Target(
(np.random.randint(50, 350), np.random.randint(350, 550)),
radius=20,
color=0x00FF00)
initial_position = np.array(start_point.position)
target = Target((np.random.randint(600, 760), np.random.randint(50, 250)),
radius=20)
objective = NavigationObjective(target, env)
env.non_interactive_objects += [start_point, target]
robot = Robot(initial_position,
cmdargs,
env,
path_color=path_color,
name=robot_name,
objective=objective)
robot.put_sensor('radar', RadarSensor(env, robot, [], radar))
robot.put_sensor('gps', GpsSensor(robot))
if robot_name in params['robots']:
robot.put_sensor('params', params['robots'][robot_name])
else:
robot.put_sensor('params', {})
robot_obstacle = DynamicObstacle(MovementPattern.RobotBodyMovement(robot))
robot_obstacle.shape = 1
robot_obstacle.radius = 10
robot_obstacle.fillcolor = (0xcc, 0x88, 0x44)
robot.set_obstacle(robot_obstacle)
return robot, target
def __init__(self, N=1000, width=1000, height=1000):
self.N = N
self.particles = [
Robot(random.random() * float(width),
random.random() * float(height),
random.random() * 2.0 * pi) for i in range(N)
]
def main():
"""
Main function
"""
try:
# Instantiate odometry. Default value will be 0,0,0
# robot = Robot(init_position=args.pos_ini)
robot = Robot()
if is_debug:
start_robot_drawer(robot.finished, robot)
else:
start_robot_logger(robot.finished, robot,
"./out/trayectoria_1.csv")
print("X value at the beginning from main X= %d" % (robot.x.value))
# 1. launch updateOdometry Process()
robot.startOdometry()
# 2. perform trajectory
path_1_odometry(robot)
# 3. wrap up and close stuff ...
# This currently unconfigure the sensors, disable the motors,
# and restore the LED to the control of the BrickPi3 firmware.
robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
robot.stopOdometry()
def main():
try:
# if args.radioD < 0:
# print('d must be a positive value')
# exit(1)
# Initialize Odometry. Default value will be 0,0,0
robot = Robot()
# 1. launch updateOdometry thread()
robot.startOdometry()
robot.catch('down')
robot.catch('up')
robot.catch('up')
robot.catch('down')
robot.catch('up')
robot.catch('down')
robot.catch('up')
robot.catch('down')
robot.catch('up')
robot.catch('down')
robot.catch('up')
# This currently unconfigure the sensors, disable the motors,
# and restore the LED to the control of the BrickPi3 firmware.
robot.stopOdometry()
except KeyboardInterrupt:
# except the program gets interrupted by Ctrl+C on the keyboard.
# THIS IS IMPORTANT if we want that motors STOP when we Ctrl+C ...
robot.stopOdometry()
def Process():
toy = Robot()
try:
print("\n")
print("Welcome to the Toy Robot application.\n")
print("Proceed to enter commands. Press CTRL-C to exit.\n")
# Loop until we receive a kill signal.
while True:
# BUG: Change to input() for Python 3.x
line = raw_input("> ")
try:
# Parse the command.
cmd = Command(line)
# Execute the validated command on the robot.
cmd.Execute(toy)
except CommandException as ce:
print(ce)
except RobotException as re:
print(re)
except CompassHeadingException as he:
print(he)
except KeyboardInterrupt as kb:
print("Keyboard exit signal detected.")
pass
finally:
print("Exit requested.")
toy.Cleanup()
print("Application successfully exited.")
return
def setupOne(self):
self.robots.append(Robot((500, 1000), 90))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 + 7 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 + 5 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 + 3 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 + Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 - Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 - 3 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 - 5 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 2 - 7 * Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
def __init__(self, robot_position, world_map, shared_map, shared_visited, robot_search_mode):
self.robot = Robot([robot_position[0], robot_position[1]], shared_map, shared_visited, robot_search_mode)
self.robot_current_position_in_world_map = [robot_position[0], robot_position[1]]
self.world_map = world_map
self.timestep = 11
self.TOTAL_WAITING_TIME = 10
self.stopped = False
def select_a_rect_distance_search(self):
self.load_environment()
self.clear_components()
self.optionDialog = DialogOption()
self.optionDialog.display_option_message("Do you want avoid cycles")
option_response = self.optionDialog.get_taken_option()
message = "Please specify the numerator and denominator to complement the heuristic method"
aSearchValueDialog = DialogASearchValues()
aSearchValueDialog.show_request_message(message)
numerator = aSearchValueDialog.get_numerator()
denominator = aSearchValueDialog.get_denominator()
self.robot = Robot(self,self.environment,self.dimension,self.queue_dimension,"A*h1",option_response,numerator,denominator)
if option_response:
self.labelSelectedSearch.setText("A* Straight Line Distance avoiding cycles")
else:
self.labelSelectedSearch.setText("A* Straight Line Distance with cycles")
self.draw_environment()
self.plainTextEditRobotStatus.appendPlainText("Ready .......")
self.pushButtonMoveRobot.setEnabled(False)
self.pushButtonLookGraph.setEnabled(True)
self.pushButtonFastSearch.setEnabled(True)
self.pushButtonNextStep.setEnabled(True)
def populate_robots(
self): # Initializes Population of robots for generation 0
""" Initializes Population of robots for generation 0 """
for i in range(self.size):
rand_bit_string = ''.join(random.choices('10', k=56))
self.robots_list.append(Robot('E', 1, 1, rand_bit_string))
Population.generation_num += 1
class Screen(object):
r = Robot()
def enableScreen(self):
print("Enable touch")
q = queue.Queue()
t1 = Thread(target=self.loopRobot, args=(q,))
t2 = Thread(target=self.r.moveRobot, args=(q,))
t1.start()
t2.start()
q.join()
def loopRobot(self, eventQ):
while 1:
try:
point, evt = eventQ.get(timeout=3)
# construct a point cannot detected
if point == 3:
time.sleep(8)
print(str.format("Screen detect touched {}", point))
evt.set()
continue
except queue.Empty:
print("Queue empty")
break
def touchDetected(self, signum, frame):
print("Screen detect touched")
def noTouchDetectedTimeout(self, signum, frame):
print("Screen no touch detected, Timeout")
def __init__(self):
self.__shapes = []
self.limit = Square("limit", np.array([[]], dtype=np.int32))
self.robot = Robot(Shape("robot", np.array([[]], dtype=np.int32)), Shape("orientation", np.array([[]], dtype=np.int32)))
self.target = None
self.treasures = []
self.orientationForTreasure = 0
def initializeRobots():
"""!
This function determines the names of each robot that should be controlled. It then creates
a @ref Robot object for each robot. Those objects perform the correct initialization of needed
parameters.
@return A list of all created Robot objects
@throw rospy.ROSInitException Thrown if no robot_list parameter is found or if no robots are
specified.
"""
# Determine the list of robots and error if not found.
robot_names = ParameterLookup.lookup('/robot_list')
# Throw an error if there are no robots specified.
if len(robot_names) < 1:
error_string = 'Must specify at least one robot in /robot_list'
rospy.logfatal(error_string)
raise rospy.ROSInitException(error_string)
# There is a chance the user specifies only a single robot without brackets, which would make
# this parameter as a string. Check for that and convert it to a list for use later.
if type(robot_names) is not list:
single_robot_name = robot_names
robot_names = [single_robot_name]
# Create each robot object and add it to a list.
robot_list = []
for name in robot_names:
robot_list.append(Robot(name))
return robot_list
def setupTwo(self):
self.robots.append(Robot((500, 1000), 90))
self.hives.append(Hive((Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT / 4 - Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
self.hives.append(Hive(
(Game.GAME_WIDTH / 2 - Game.BLOCK_SIZE / 2, Game.GAME_HEIGHT - Game.GAME_HEIGHT / 4 + Game.BLOCK_SIZE / 2),
HiveType.HEALTHY))
def particleFilter(motions, measurements, N=numbOfParticles):
# 1) make particles
particles = [
Robot({
'landmarks': landmarks,
'worldSize': worldSize
}) for particle in range(numbOfParticles)
]
[
particle.set_noise({
'bearing': bearing_noise,
'steering': steering_noise,
'distance': distance_noise
}) for particle in particles
]
for motion, measurement in zip(motions, measurements):
particles = [particle.move(motion) for particle in particles]
particleWeights = [
particle.measurement_prob(measurement) for particle in particles
]
particleWeights = map(lambda weight: weight / sum(particleWeights),
particleWeights)
particles = np.random.choice(a=particles,
size=len(particles),
replace=True,
p=particleWeights)
return extractPositions(particles)
def test_raise_engage_length_value_error(self):
# it should pass only if the result is raising a ValueError
world_size = (5, 4)
lost_robots = []
with self.assertRaises(ValueError):
robot = Robot(world_size, lost_robots, 10, 10, 'N')
robot.engage('k' * 102)
def main():
Pioneer = Robot(clientID)
Pioneer.turnLeft()
time.sleep(2)
Pioneer.moveForward()
time.sleep(2)
Pioneer.turnRight()
time.sleep(2)
Pioneer.moveBackward()
time.sleep(2)
Pioneer.moveForward()
time.sleep(6)
Pioneer.stop()
# errorCode, left_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_oneshot_wait)
# errorCode, right_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_oneshot_wait)
# errorCode=sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode=sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode, sensor1 = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor1', sim.simx_opmode_oneshot_wait)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_streaming)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_buffer)
errorCam0, camera_handle = sim.simxGetObjectHandle(
clientID, cam_name, sim.simx_opmode_oneshot_wait)
# Start the Stream
errorCam1, res, image = sim.simxGetVisionSensorImage(
clientID, camera_handle, 0, sim.simx_opmode_streaming)
def run_training(maze_size=(6, 6),
trap_number=1,
epoch=20,
epsilon0=0.3,
alpha=0.3,
gamma=0.9):
# # 可选的参数：
# epoch = 20
# # 随机探索的初始概率
# epsilon0 = 0.3
# # 松弛变量
# alpha = 0.3
# # 折扣因子
# gamma = 0.9
# # 地图大小
# maze_size = (6, 6)
# # 陷阱数量
# trap_number = 1
g = Maze(maze_size=maze_size, trap_number=trap_number)
r = Robot(g, alpha=alpha, epsilon0=epsilon0, gamma=gamma)
r.set_status(learning=True)
runner = Runner(r, g)
runner.run_training(epoch, display_direction=True)
# runner.generate_movie(filename = "final1.mp4") # 你可以注释该行代码，加快运行速度，不过你就无法观察到视频了。
# runner.plot_results()
return runner
def test_generate_sample_variable_sample_size(self):
size_x = 5
obstacle_rate = 0.25
r = Robot()
r.set_size(size_x)
r.set_error(0.05)
r.set_obstacle_rate(obstacle_rate)
r.generate_map()
r.make_a_mat()
r.make_pi_v()
r.make_b_mat()
for sample_size in range(1, 10):
sam_s, sam_o = r.generate_sample(sample_size)
self.assertEqual(sam_s.size, sample_size)
self.assertEqual(sam_o.size, sample_size)
states_count = r.pi_v.size
self.assertTrue(
(0 <= sam_s).all() and (sam_s < states_count).all(),
"States sample contains unknown states: {}".format(sam_s))
self.assertTrue(
(0 <= sam_o).all() and (sam_o < self._OBSERVATION_COUNT).all(),
"Observations sample contains unknown observations: {}".format(
sam_o))
for i in range(sample_size - 1):
state_transition_probability = r._get_state_transition_probability(
sam_s[i + 1], sam_s[i])
self.assertTrue(0 < state_transition_probability <= 1)
def main():
robot = Robot()
canvas = Canvas()
navigator = WaypointNavigator(robot);
while True:
navigator.navigateToWaypoint(*getWayPoint())
