diff --git a/behavior_metrics/brains/brains_handler.py b/behavior_metrics/brains/brains_handler.py
index fff507fa..3ac8dfbc 100755
--- a/behavior_metrics/brains/brains_handler.py
+++ b/behavior_metrics/brains/brains_handler.py
@@ -3,7 +3,7 @@
import subprocess
import os
import traceback
-
+from utils.logger import logger
from abc import abstractmethod
from albumentations import (
Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare, Affine
@@ -38,15 +38,14 @@ def load_brain(self, path, model=None):
robot_type = path_split[-2]
module_name = path_split[-1][:-3] # removing .py extension
- # import_name = 'brains.' + framework + '.' + robot_type + '.' + module_name
if len(path_split) == 4:
- framework = path_split[2]
- import_name = 'brains.' + robot_type + '.' + framework + '.' + module_name
+ import_name = 'brains.' + framework + '.' + robot_type + '.' + module_name
else:
import_name = 'brains.' + robot_type + '.' + module_name
- print(import_name)
+ logger.info("import_name:" + import_name)
+
if robot_type == 'CARLA':
module = importlib.import_module(import_name)
Brain = getattr(module, 'Brain')
@@ -61,6 +60,8 @@ def load_brain(self, path, model=None):
if import_name in sys.modules: # for reloading sake
del sys.modules[import_name]
module = importlib.import_module(import_name)
+ Brain = getattr(module, 'Brain')
+ print('Brain: ', Brain)
if robot_type == 'drone':
self.active_brain = Brain(handler=self, config=self.config)
else:
diff --git a/behavior_metrics/configs/CARLA/default_carla_ros2.yml b/behavior_metrics/configs/CARLA/default_carla_ros2.yml
index 0d4efbeb..d8a27024 100644
--- a/behavior_metrics/configs/CARLA/default_carla_ros2.yml
+++ b/behavior_metrics/configs/CARLA/default_carla_ros2.yml
@@ -52,7 +52,7 @@ Behaviors:
World: configs/CARLA/CARLA_launch_files/town_01_anticlockwise.launch.py
RandomSpawnPoint: False
Dataset:
- In: '/tmp/my_bag/'
+ In: '/tmp/my_bag'
Out: ''
Stats:
Out: './'
diff --git a/behavior_metrics/configs/gazebo/default_ros2.yml b/behavior_metrics/configs/gazebo/default_ros2.yml
new file mode 100644
index 00000000..d3af09b4
--- /dev/null
+++ b/behavior_metrics/configs/gazebo/default_ros2.yml
@@ -0,0 +1,49 @@
+Behaviors:
+ Robot:
+ Sensors:
+ Cameras:
+ Camera_0:
+ Name: 'camera_0'
+ Topic: '/cam_f1_left/image_raw'
+ Pose3D:
+ Pose3D_0:
+ Name: 'pose3d_0'
+ Topic: '/odom'
+ Actuators:
+ Motors:
+ Motors_0:
+ Name: 'motors_0'
+ Topic: '/cmd_vel'
+ MaxV: 3
+ MaxW: 0.3
+
+ BrainPath: 'brains/gazebo/f1/brain_f1_dummy.py'
+ PilotTimeCycle: 50
+ Parameters:
+ ImageTranform: ''
+ Type: 'f1'
+ Simulation:
+ World: configs/gazebo/gazebo_launch_files/simple_circuit.launch.py
+ Dataset:
+ In: '/tmp/my_bag'
+ Out: ''
+ Stats:
+ Out: './'
+ PerfectLap: './perfect_bags/lap-simple-circuit.bag'
+ Layout:
+ Frame_0:
+ Name: frame_0
+ Geometry: [1, 1, 2, 2]
+ Data: rgbimage
+ Frame_1:
+ Name: frame_1
+ Geometry: [0, 1, 1, 1]
+ Data: rgbimage
+ Frame_2:
+ Name: frame_2
+ Geometry: [0, 2, 1, 1]
+ Data: rgbimage
+ Frame_3:
+ Name: frame_3
+ Geometry: [0, 3, 3, 1]
+ Data: rgbimage
diff --git a/behavior_metrics/configs/gazebo/gazebo_launch_files/simple_circuit.launch.py b/behavior_metrics/configs/gazebo/gazebo_launch_files/simple_circuit.launch.py
new file mode 100755
index 00000000..4de98e33
--- /dev/null
+++ b/behavior_metrics/configs/gazebo/gazebo_launch_files/simple_circuit.launch.py
@@ -0,0 +1,79 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+ # Set the path to the Gazebo ROS package
+ pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
+
+ # Set the path to this package.
+ pkg_share = FindPackageShare(package='f1_cars').find('f1_cars')
+
+ # Set the path to the world file
+ world_file_name = 'simple_circuit.world'
+ world_path = os.path.join(pkg_share, 'worlds', world_file_name)
+
+ # Set the path to the SDF model files.
+ gazebo_models_path = os.path.join(pkg_share, 'models')
+ os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path
+
+ ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############
+ # Launch configuration variables specific to simulation
+ headless = LaunchConfiguration('headless')
+ use_sim_time = LaunchConfiguration('use_sim_time')
+ use_simulator = LaunchConfiguration('use_simulator')
+ world = LaunchConfiguration('world')
+
+ declare_simulator_cmd = DeclareLaunchArgument(
+ name='headless',
+ default_value='False',
+ description='Whether to execute gzclient')
+
+ declare_use_sim_time_cmd = DeclareLaunchArgument(
+ name='use_sim_time',
+ default_value='true',
+ description='Use simulation (Gazebo) clock if true')
+
+ declare_use_simulator_cmd = DeclareLaunchArgument(
+ name='use_simulator',
+ default_value='True',
+ description='Whether to start the simulator')
+
+ declare_world_cmd = DeclareLaunchArgument(
+ name='world',
+ default_value=world_path,
+ description='Full path to the world model file to load')
+
+ # Specify the actions
+
+ # Start Gazebo server
+ start_gazebo_server_cmd = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+ condition=IfCondition(use_simulator),
+ launch_arguments={'world': world}.items())
+
+ # Start Gazebo client
+ start_gazebo_client_cmd = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
+ condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))
+
+ # Create the launch description and populate
+ ld = LaunchDescription()
+
+ # Declare the launch options
+ ld.add_action(declare_simulator_cmd)
+ ld.add_action(declare_use_sim_time_cmd)
+ ld.add_action(declare_use_simulator_cmd)
+ ld.add_action(declare_world_cmd)
+
+ # Add any actions
+ ld.add_action(start_gazebo_server_cmd)
+ ld.add_action(start_gazebo_client_cmd)
+
+ return ld
\ No newline at end of file
diff --git a/behavior_metrics/driver_gazebo.py b/behavior_metrics/driver_gazebo.py
index f5a81b38..c0a170d8 100644
--- a/behavior_metrics/driver_gazebo.py
+++ b/behavior_metrics/driver_gazebo.py
@@ -79,8 +79,13 @@ def check_args(argv):
Colors.OKBLUE, Colors.ENDC))
args = parser.parse_args()
+ # get ROS version from environment variable
+ ros_version = os.environ.get('ROS_VERSION', '2') # Default is ROS 2
+ if ros_version not in ['1', '2']:
+ logger.error('Invalid ROS_VERSION environment variable. Must be "1" or "2". Killing program...')
+ sys.exit(-1)
- config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False}
+ config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False, 'ros_version': int(ros_version)}
if args.config:
config_data['config'] = []
for config_file in args.config:
@@ -104,6 +109,31 @@ def check_args(argv):
return config_data
+def init_node(ros_version):
+ """
+ Initializes the ROS node based on the selected version.
+
+ Arguments:
+ ros_version (str): The ROS version ("ros1" or "ros2").
+
+ Returns:
+ For ROS1: returns None (as rospy manages the node globally).
+ For ROS2: returns the created node.
+ """
+ if ros_version == 1:
+ import rospy
+ rospy.init_node('my_ros1_node')
+ return None # rospy maneja la instancia globalmente
+ elif ros_version == 2:
+ import rclpy
+ rclpy.init()
+ node = rclpy.create_node('my_ros2_node')
+ logger.info('ROS2 node initialized')
+ return node
+ else:
+ logger.error(f"Unsupported ROS version: {ros_version}")
+ sys.exit(-1)
+
def conf_window(configuration):
"""Gui windows for configuring the app. If not configuration file specified when launched, this windows appears,
otherwise, main_win is called.
@@ -132,7 +162,7 @@ def conf_window(configuration):
pass
-def main_win(configuration, controller):
+def main_win(configuration, controller, node):
"""shows the Qt main window of the application
Arguments:
@@ -146,7 +176,7 @@ def main_win(configuration, controller):
app = QApplication(sys.argv)
main_window = ParentWindow()
- views_controller = ViewsController(main_window, configuration, controller)
+ views_controller = ViewsController(main_window, configuration, controller, node)
views_controller.show_main_view(True)
main_window.show()
@@ -161,11 +191,13 @@ def main():
# Check and generate configuration
config_data = check_args(sys.argv)
+ node = init_node(config_data['ros_version']) # Initialize the ROS node shared by all the application
+
for config in config_data['config']:
app_configuration = Config(config)
# Create controller of model-view
- controller = ControllerGazebo()
+ controller = ControllerGazebo(node)
# If there's no config, configure the app through the GUI
if app_configuration.empty and config_data['gui']:
@@ -194,14 +226,15 @@ def main():
if not config_data['script']:
# Launch control
- pilot = PilotGazebo(app_configuration, controller, app_configuration.brain_path)
+
+ pilot = PilotGazebo(node, app_configuration, controller, app_configuration.brain_path)
pilot.daemon = True
pilot.start()
logger.info('Executing app')
# If GUI specified, launch it. Otherwise don't
if config_data['gui']:
- main_win(app_configuration, controller)
+ main_win(app_configuration, controller,node)
else:
pilot.join()
diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py
index f1e79493..4c033fb9 100644
--- a/behavior_metrics/pilot_carla.py
+++ b/behavior_metrics/pilot_carla.py
@@ -42,8 +42,6 @@
__license__ = 'GPLv3'
-
-
class PilotCarla(threading.Thread):
"""This class handles the robot and its brain.
diff --git a/behavior_metrics/pilot_gazebo.py b/behavior_metrics/pilot_gazebo.py
index 8c756829..23d18735 100644
--- a/behavior_metrics/pilot_gazebo.py
+++ b/behavior_metrics/pilot_gazebo.py
@@ -12,6 +12,8 @@
this program. If not, see .
"""
+import os
+import threading
import threading
import time
@@ -55,14 +57,14 @@ class PilotGazebo(threading.Thread):
brains {brains.brains_handler.Brains} -- Brains controller instance
"""
- def __init__(self, configuration, controller, brain_path):
+ def __init__(self, node, configuration, controller, brain_path):
"""Constructor of the pilot class
Arguments:
configuration {utils.configuration.Config} -- Configuration instance of the application
controller {utils.controller.Controller} -- Controller instance of the MVC of the application
"""
-
+ self.node = node
self.controller = controller
self.controller.set_pilot(self)
self.configuration = configuration
@@ -107,9 +109,10 @@ def __wait_gazebo(self):
def initialize_robot(self):
"""Initialize robot interfaces (sensors and actuators) and its brain from configuration"""
self.stop_interfaces()
+
if self.robot_type != 'drone':
- self.actuators = Actuators(self.configuration.actuators)
- self.sensors = Sensors(self.configuration.sensors)
+ self.actuators = Actuators(self.configuration.actuators,self.node)
+ self.sensors = Sensors(self.configuration.sensors,self.node)
if hasattr(self.configuration, 'experiment_model') and type(self.configuration.experiment_model) != list:
self.brains = Brains(self.sensors, self.actuators, self.brain_path, self.controller,
self.configuration.experiment_model, self.configuration.brain_kwargs)
@@ -163,7 +166,7 @@ def run(self):
self.brain_iterations_simulated_time.append(self.ros_clock_time - start_time_ros)
self.execution_completed = True
if ros_version == '2':
- self.ros2_node.destroy_subscription(self.clock_subscriber)
+ self.node.destroy_subscription(self.clock_subscriber)
else:
self.clock_subscriber.unregister()
self.stats_process.terminate()
@@ -290,10 +293,8 @@ def track_stats(self):
time.sleep(1)
poll = self.stats_process.poll()
-
if ros_version == '2':
- self.ros2_node = Node("pilot_gazebo")
- self.clock_subscriber = self.ros2_node.create_subscription(Clock, "/clock", self.clock_callback,1)
+ self.clock_subscriber = self.node.create_subscription(Clock, "/clock", self.clock_callback,1)
else:
self.clock_subscriber = rospy.Subscriber("/clock", Clock, self.clock_callback)
diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py
index d482cb88..97d4f439 100644
--- a/behavior_metrics/ui/gui/views/toolbar.py
+++ b/behavior_metrics/ui/gui/views/toolbar.py
@@ -487,6 +487,7 @@ def create_brain_gb(self):
brain_label.setMaximumWidth(100)
if self.configuration.brain_path:
current_brain = self.configuration.brain_path.split('/')[-1].split(".")[0] # get brain name without .py
+
else:
current_brain = ''
self.current_brain_label = QLabel('Current brain: ' + current_brain + '')
@@ -496,9 +497,15 @@ def create_brain_gb(self):
self.brain_combobox = QComboBox()
self.brain_combobox.setEnabled(True)
environment_subpath = self.configuration.environment + '/' if self.configuration.environment is not None else ""
- brains = [file.split(".")[0] for file
- in os.listdir(brains_path + environment_subpath + self.configuration.robot_type)
- if file.endswith('.py') and file.split(".")[0] != '__init__']
+
+ if self.configuration.robot_type == "CARLA":
+ brains = [file.split(".")[0] for file
+ in os.listdir(brains_path + environment_subpath + self.configuration.robot_type)
+ if file.endswith('.py') and file.split(".")[0] != '__init__']
+ else:
+ brains = [file.split(".")[0] for file
+ in os.listdir(brains_path + environment_subpath + '/gazebo/' + self.configuration.robot_type)
+ if file.endswith('.py') and file.split(".")[0] != '__init__']
self.brain_combobox.addItem('')
self.brain_combobox.addItems(brains)
index = self.brain_combobox.findText(current_brain, Qt.MatchFixedString)
diff --git a/behavior_metrics/utils/controller_gazebo.py b/behavior_metrics/utils/controller_gazebo.py
index 3cabe8dc..9ae3dddc 100644
--- a/behavior_metrics/utils/controller_gazebo.py
+++ b/behavior_metrics/utils/controller_gazebo.py
@@ -20,10 +20,10 @@
import subprocess
import threading
import cv2
-import rospy
+#import rospy
import os
import time
-import rosbag
+#import rosbag
import json
from std_srvs.srv import Empty
@@ -35,6 +35,14 @@
from std_msgs.msg import String
from utils import metrics_gazebo
+ros_version = os.environ.get('ROS_VERSION', '2')
+if ros_version == "2":
+ import rclpy
+ from rclpy.node import Node
+else:
+ import rospy
+ import rosbag
+
__author__ = 'fqez'
__contributors__ = []
__license__ = 'GPLv3'
@@ -51,9 +59,10 @@ class ControllerGazebo:
recording {bool} -- Flag to determine if a rosbag is being recorded
"""
- def __init__(self):
+ def __init__(self, node: Node):
""" Constructor of the class. """
pass
+ self.node = node
self.__data_loc = threading.Lock()
self.__pose_loc = threading.Lock()
self.data = {}
@@ -122,23 +131,43 @@ def get_pose3D(self):
def reset_gazebo_simulation(self):
logger.info("Restarting simulation")
- reset_physics = rospy.ServiceProxy('/gazebo/reset_world', Empty)
- reset_physics()
+ if ros_version == "2":
+ reset_physics = self.node.create_client(Empty, '/gazebo/reset_world')
+ else:
+ reset_physics = rospy.ServiceProxy('/gazebo/reset_world', Empty)
+ if ros_version == "2":
+ reset_physics.call_async(None)
+ else:
+ reset_physics()
def pause_gazebo_simulation(self):
logger.info("Pausing simulation")
- pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
+ if ros_version == "2":
+ pause_physics = self.node.create_client(Empty,'/gazebo/pause_physics')
+ else:
+ pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
try:
- pause_physics()
+ if ros_version == "2":
+ pause_physics.call_async(None)
+ else:
+ pause_physics()
except Exception as ex:
print(ex)
self.pilot.stop_event.set()
def unpause_gazebo_simulation(self):
logger.info("Resuming simulation")
- unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+
+ if ros_version == "2":
+ unpause_physics = self.node.create_client(Empty, '/gazebo/unpause_physics')
+ else:
+ unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
try:
- unpause_physics()
+ if ros_version == "2":
+ unpause_physics.call_async(None)
+ else:
+ unpause_physics()
+
except Exception as ex:
print(ex)
self.pilot.stop_event.clear()
@@ -150,35 +179,44 @@ def record_rosbag(self, topics, dataset_name):
topics {list} -- List of topics to be recorde
dataset_name {str} -- Path of the resulting bag file
"""
-
- if not self.recording:
- logger.info("Recording bag at: {}".format(dataset_name))
- self.recording = True
-
- if ros_version == "2":
- command = "ros2 bag record -o " + dataset_name + "/behav_bag" + " " + " ".join(topics)
- else:
- command = "rosbag record -O " + dataset_name + " " + " ".join(topics) + " __name:=behav_bag"
-
- logger.info("Recording bag at: {}".format(dataset_name))
- cmd_split = shlex.split(command)
- with open("./logs/.roslaunch_stdout.log", "w") as out, open("./logs/.roslaunch_stderr.log", "w") as err:
- self.rosbag_proc = subprocess.Popen(cmd_split, stdout=out, stderr=err)
- else:
+ if self.recording:
logger.info("Rosbag already recording")
self.stop_record()
+ return
+
+ self.recording = True
+
+ if ros_version == "2":
+ command = "ros2 bag record -o " + dataset_name + "/behav_bag" + " " + " ".join(topics)
+ else:
+ command = "rosbag record -O " + dataset_name + " " + " ".join(topics) + " __name:=behav_bag"
+
+ logger.info("Recording bag at: {}".format(dataset_name))
+ cmd_split = shlex.split(command)
+ with open("./logs/.roslaunch_stdout.log", "w") as out, open("./logs/.roslaunch_stderr.log", "w") as err:
+ self.rosbag_proc = subprocess.Popen(cmd_split, stdout=out, stderr=err)
+
+
+
def stop_record(self):
"""Stop the rosbag recording process."""
- if self.rosbag_proc and self.recording:
- logger.info("Stopping bag recording")
- self.recording = False
+ if not self.recording or not self.rosbag_proc:
+ logger.info("No bag recording")
+ return
+
+ if ros_version == "2":
+ self.rosbag_proc.terminate()
+ self.rosbag_proc.wait()
+ logger.info("Stopped bag recording")
+ else:
command = "rosnode kill /behav_bag"
- command = shlex.split(command)
+ command_split = shlex.split(command)
with open("./logs/.roslaunch_stdout.log", "w") as out, open("./logs/.roslaunch_stderr.log", "w") as err:
- subprocess.Popen(command, stdout=out, stderr=err)
- else:
- logger.info("No bag recording")
+ subprocess.Popen(command_split, stdout=out, stderr=err)
+ self.recording = False
+ self.rosbag_proc = None
+
def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None):
logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path))
@@ -212,7 +250,12 @@ def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_co
time_str = time.strftime("%Y%m%d-%H%M%S")
self.experiment_metrics_filename = time_str + '.bag'
topics = ['/F1ROS/odom', '/clock']
- command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag"
+
+ if ros_version == "2":
+ command = "ros2 bag record -o " + self.experiment_metrics_bag_filename + " " + " ".join(topics)
+ else:
+ command = "rosbag record -O " + self.experiment_metrics_bag_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag"
+
command = shlex.split(command)
with open("./logs/.roslaunch_stdout.log", "w") as out, open("./logs/.roslaunch_stderr.log", "w") as err:
self.proc = subprocess.Popen(command, stdout=out, stderr=err)
@@ -221,7 +264,10 @@ def stop_recording_metrics(self, pitch_error=False):
logger.info("Stopping metrics bag recording")
end_time = time.time()
- command = "rosnode kill /behav_metrics_bag"
+ if ros_version == "2":
+ command = "ros2 node kill /behav_metrics_bag"
+ else:
+ command = "rosnode kill /behav_metrics_bag"
command = shlex.split(command)
with open("./logs/.roslaunch_stdout.log", "w") as out, open("./logs/.roslaunch_stderr.log", "w") as err:
subprocess.Popen(command, stdout=out, stderr=err)
diff --git a/behavior_metrics/utils/script_manager_gazebo.py b/behavior_metrics/utils/script_manager_gazebo.py
index 361266cb..271d3508 100644
--- a/behavior_metrics/utils/script_manager_gazebo.py
+++ b/behavior_metrics/utils/script_manager_gazebo.py
@@ -19,7 +19,6 @@
import subprocess
import time
import os
-import rospy
import random
import sys
import matplotlib.pyplot as plt
@@ -31,6 +30,13 @@
from pilot_gazebo import PilotGazebo
from utils.tmp_world_generator import tmp_world_generator
+ros_version = os.environ.get('ROS_VERSION', '2')
+if ros_version == '2':
+ import rclpy
+ from rclpy.node import Node
+else:
+ import rospy
+
def run_brains_worlds(app_configuration, controller, randomize=False):
worlds = enumerate(app_configuration.current_world)
worlds_list = list(worlds)
@@ -104,7 +110,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False):
experiment_timeout = CIRCUITS_TIMEOUTS[os.path.basename(world)] * 1.1
while (controller.pilot.ros_clock_time - time_start < experiment_timeout and not is_finished) \
or controller.pilot.ros_clock_time - time_start < 20:
- rospy.sleep(10)
+ time.sleep(10)
old_point = new_point
new_point = np.array([controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().x,
controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().y])