Merge pull request #711 from Amglega/noetic-devel

Added code for ROS2 gazebo migration

Author: Amglega

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:

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: './'

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

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

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

behavior_metrics/pilot_carla.py

@@ -42,8 +42,6 @@
 __license__ = 'GPLv3'
 
 
-
-
 class PilotCarla(threading.Thread):
     """This class handles the robot and its brain.
 

behavior_metrics/pilot_gazebo.py

@@ -12,6 +12,8 @@
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
 
+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)
 

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: <b><FONT COLOR = lightgreen>' + current_brain + '</b>')
@@ -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)