Merge pull request #315 from JdeRobot/issue-314

Updated random orientation calculation

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'

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

behavior_metrics/utils/random_initializer.py → behavior_metrics/utils/tmp_world_generator.py

@@ -24,6 +24,7 @@
 import rospy
 import random
 import sys
+import math
 
 import numpy as np
 
@@ -32,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)
@@ -54,41 +55,48 @@ 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]
 
+        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
+
+        # Half chances of orientating the car to the exactly opposite direction
         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']
-    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
+            result = (result + 180) % 360
+
+        radians = math.radians(result)
+        orientation_z = radians
+
+        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: