From 7bf98fa419e3a56a45cab85cb1cc7f7b643085eb Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Tue, 8 Feb 2022 17:04:44 +0100 Subject: [PATCH 1/4] Updated random orientation calculation --- behavior_metrics/utils/random_initializer.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/behavior_metrics/utils/random_initializer.py b/behavior_metrics/utils/random_initializer.py index a82e7163..2d2fa3dc 100644 --- a/behavior_metrics/utils/random_initializer.py +++ b/behavior_metrics/utils/random_initializer.py @@ -24,6 +24,7 @@ import rospy import random import sys +import math import numpy as np @@ -54,14 +55,20 @@ def tmp_random_initializer(current_world, stats_perfect_lap, real_time_update_ra perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(stats_perfect_lap) if randomize: - random_index = random.randint(0, int(len(perfect_lap_checkpoints) / 2)) + random_index = random.randint(0, int(len(perfect_lap_checkpoints))) random_point = perfect_lap_checkpoints[random_index] - random_orientation = random.randint(0, 1) - if random_orientation == 1: - orientation_z = random_point['pose.pose.orientation.z'] + np.random.normal(0, 0.1) - else: - orientation_z = random_point['pose.pose.orientation.z'] + p1 = perfect_lap_checkpoints[random_index] + p2 = perfect_lap_checkpoints[random_index + 5] + delta_y = p2['pose.pose.position.y'] - p1['pose.pose.position.y'] + delta_x = p2['pose.pose.position.x'] - p1['pose.pose.position.x'] + result = math.atan2(delta_y, delta_x) + result = math.degrees(result) + if result < 0: + result = 360 + result + result = (result + 180) % 360 + radians = math.radians(result) + orientation_z = radians else: random_index = 0 random_point = perfect_lap_checkpoints[random_index] From f44b5755d769539ed1942268d37e40c57ba4da2b Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Tue, 8 Feb 2022 17:17:47 +0100 Subject: [PATCH 2/4] Exactly opposite direction depending on random value --- behavior_metrics/utils/random_initializer.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/behavior_metrics/utils/random_initializer.py b/behavior_metrics/utils/random_initializer.py index 2d2fa3dc..f9ae2a0b 100644 --- a/behavior_metrics/utils/random_initializer.py +++ b/behavior_metrics/utils/random_initializer.py @@ -66,7 +66,12 @@ def tmp_random_initializer(current_world, stats_perfect_lap, real_time_update_ra result = math.degrees(result) if result < 0: result = 360 + result - result = (result + 180) % 360 + + # Half chances of orientating the car to the exactly opposite direction + random_orientation = random.randint(0, 1) + if random_orientation == 1: + result = (result + 180) % 360 + radians = math.radians(result) orientation_z = radians else: From 8f9a604a52c7eb47e77787a748ae9ae67d82cb12 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Tue, 8 Feb 2022 17:30:54 +0100 Subject: [PATCH 3/4] Random init point only if parameter is present --- behavior_metrics/utils/random_initializer.py | 40 +++++++++----------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/behavior_metrics/utils/random_initializer.py b/behavior_metrics/utils/random_initializer.py index f9ae2a0b..811a747a 100644 --- a/behavior_metrics/utils/random_initializer.py +++ b/behavior_metrics/utils/random_initializer.py @@ -74,33 +74,29 @@ def tmp_random_initializer(current_world, stats_perfect_lap, real_time_update_ra radians = math.radians(result) orientation_z = radians - else: - random_index = 0 - random_point = perfect_lap_checkpoints[random_index] - orientation_z = random_point['pose.pose.orientation.z'] - - random_start_point = np.array( - [round(random_point['pose.pose.position.x'], 3), round(random_point['pose.pose.position.y'], 3), - round(random_point['pose.pose.position.z'], 3), round(random_point['pose.pose.orientation.x'], 3), - round(random_point['pose.pose.orientation.y'], 3), round(orientation_z, 3)]) - - for child_1 in root[0]: - if child_1.tag == 'include': - next = False - for child_2 in child_1: - if next: - child_2.text = str(random_start_point[0]) + " " + str(random_start_point[1]) + " " + str( - random_start_point[2]) + " " + str(random_start_point[3]) + " " + str( - random_start_point[4]) + " " + str(random_start_point[5]) - next = False - elif child_2.text == 'model://f1_renault': - next = True + + random_start_point = np.array( + [round(random_point['pose.pose.position.x'], 3), round(random_point['pose.pose.position.y'], 3), + round(random_point['pose.pose.position.z'], 3), round(random_point['pose.pose.orientation.x'], 3), + round(random_point['pose.pose.orientation.y'], 3), round(orientation_z, 3)]) + + for child_1 in root[0]: + if child_1.tag == 'include': + next = False + for child_2 in child_1: + if next: + child_2.text = str(random_start_point[0]) + " " + str(random_start_point[1]) + " " + str( + random_start_point[2]) + " " + str(random_start_point[3]) + " " + str( + random_start_point[4]) + " " + str(random_start_point[5]) + next = False + elif child_2.text == 'model://f1_renault': + next = True # Add physics real time update rate value physics_element = ET.SubElement(root[0], 'physics') physics_element.set("type", "ode") real_time_update_rate_element = ET.SubElement(physics_element, 'real_time_update_rate') - real_time_update_rate_element.text = str(real_time_update_rate) # 1000 es the default value + real_time_update_rate_element.text = str(real_time_update_rate) # 1000 is the default value tree.write('tmp_world.launch') if launch: From 46f5b53e6d641d3c6c3f5a629bc7c66869a2350b Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Tue, 8 Feb 2022 17:49:13 +0100 Subject: [PATCH 4/4] Updated random initializer to world generator --- behavior_metrics/driver.py | 4 ++-- behavior_metrics/utils/script_manager.py | 4 ++-- .../utils/{random_initializer.py => tmp_world_generator.py} | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) rename behavior_metrics/utils/{random_initializer.py => tmp_world_generator.py} (97%) diff --git a/behavior_metrics/driver.py b/behavior_metrics/driver.py index cda07220..fbe4703c 100644 --- a/behavior_metrics/driver.py +++ b/behavior_metrics/driver.py @@ -26,7 +26,7 @@ from utils.configuration import Config from utils.controller import Controller from utils.logger import logger -from utils.random_initializer import tmp_random_initializer +from utils.tmp_world_generator import tmp_world_generator __author__ = 'fqez' __contributors__ = [] @@ -175,7 +175,7 @@ def main(): if app_configuration.current_world and not config_data['script']: logger.debug('Launching Simulation... please wait...') if config_data['random']: - tmp_random_initializer(app_configuration.current_world, app_configuration.stats_perfect_lap, + tmp_world_generator(app_configuration.current_world, app_configuration.stats_perfect_lap, app_configuration.real_time_update_rate, randomize=True, gui=True, launch=False) app_configuration.current_world = 'tmp_circuit.launch' diff --git a/behavior_metrics/utils/script_manager.py b/behavior_metrics/utils/script_manager.py index 222b8aae..322a3c94 100644 --- a/behavior_metrics/utils/script_manager.py +++ b/behavior_metrics/utils/script_manager.py @@ -29,7 +29,7 @@ from utils.logger import logger from utils.constants import MIN_EXPERIMENT_PERCENTAGE_COMPLETED, CIRCUITS_TIMEOUTS from pilot import Pilot -from utils.random_initializer import tmp_random_initializer +from utils.tmp_world_generator import tmp_world_generator from rosgraph_msgs.msg import Clock @@ -39,7 +39,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): for brain_counter, brain in enumerate(app_configuration.brain_path): repetition_counter = 0 while repetition_counter < app_configuration.experiment_repetitions: - tmp_random_initializer(world, app_configuration.stats_perfect_lap[world_counter], + tmp_world_generator(world, app_configuration.stats_perfect_lap[world_counter], app_configuration.real_time_update_rate, randomize=randomize, gui=False, launch=True) pilot = Pilot(app_configuration, controller, app_configuration.brain_path[brain_counter]) diff --git a/behavior_metrics/utils/random_initializer.py b/behavior_metrics/utils/tmp_world_generator.py similarity index 97% rename from behavior_metrics/utils/random_initializer.py rename to behavior_metrics/utils/tmp_world_generator.py index 811a747a..95b81450 100644 --- a/behavior_metrics/utils/random_initializer.py +++ b/behavior_metrics/utils/tmp_world_generator.py @@ -33,7 +33,7 @@ from utils.logger import logger -def tmp_random_initializer(current_world, stats_perfect_lap, real_time_update_rate, randomize=False, gui=False, +def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, randomize=False, gui=False, launch=False): environment.close_gazebo() tree = ET.parse(current_world)