Ros2 executive

astrobee/config/robots/sim.config

@@ -108,7 +108,7 @@ agent_name = "sim"
 
 heartbeat_queue_size = 20
 
-nodes_not_running = {"light_flow"}
+nodes_not_running = {"dds_ros_bridge", "astrobee_astrobee_bridge", "light_flow", "data_bagger", "handrail_detect", "image_sampler", "pico_driver", "planner_qp"}
 
 bags_save_directory = "/tmp/bags"
 

astrobee/config/simulation/simulation.config

@@ -28,7 +28,7 @@ disable_cameras_on_speedup = true;
 -- cost. Setting the rate to zero effectively disables the camera. By default
 -- we only enable the haz_cam, because it's needed for obstacle detection.
 
-nav_cam_rate   = 0.0;
-dock_cam_rate  = 0.0;
+nav_cam_rate   = 1.0;
+dock_cam_rate  = 1.0;
 perch_cam_rate = 5.0;
 haz_cam_rate   = 5.0;

astrobee/launch/astrobee.launch.py

@@ -17,7 +17,8 @@
 
 
 from utilities.utilities import *
-
+from launch.actions import GroupAction
+from launch_ros.actions import PushRosNamespace
 
 def generate_launch_description():
     robot_description = Command(['xacro ', get_path('urdf/model.urdf.xacro', 'description'),
@@ -63,66 +64,84 @@ def generate_launch_description():
         # Set the TF prefix, create a robot description and joint state publisher
         Node(
             package="robot_state_publisher",
-            namespace="",
+            namespace=LaunchConfiguration("ns"),
             executable="robot_state_publisher",
             name="robot_state_publisher",
-            parameters=[{'robot_description': ParameterValue(robot_description) }],
+            parameters=[
+              {'use_sim_time': True},
+              {'robot_description': ParameterValue(robot_description) }
+            ],
         ),
 
         # If we need to load synthetic drivers (we are not running on a real robot)
         # TODO(asymingt) - pass nodes, spurn and extra into gazebo
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/synthetic.launch.py"),
-            launch_arguments={
-                "world": LaunchConfiguration("world"),     # Don't start driver nodes
-                "ns"   : LaunchConfiguration("ns"),        # Prevent node
-                "sim"  : LaunchConfiguration("sim"),       # Launch node group
-                "pose" : LaunchConfiguration("pose"),      # Inject extra nodes
-                "bag"  : LaunchConfiguration("bag"),       # Debug a node set
-                "robot_description"  : robot_description,  # Robot description
-            }.items(), 
-            condition=UnlessCondition(LaunchConfiguration("drivers"))
+        GroupAction(
+          actions=[
+            PushRosNamespace(LaunchConfiguration('ns')),
+            IncludeLaunchDescription(
+                get_launch_file("launch/controller/synthetic.launch.py"),
+                launch_arguments={
+                    "world": LaunchConfiguration("world"),     # Don't start driver nodes
+                    "ns"   : LaunchConfiguration("ns"),        # Prevent node
+                    "sim"  : LaunchConfiguration("sim"),       # Launch node group
+                    "pose" : LaunchConfiguration("pose"),      # Inject extra nodes
+                    "bag"  : LaunchConfiguration("bag"),       # Debug a node set
+                    "robot_description"  : robot_description,  # Robot description
+                }.items(), 
+                condition=UnlessCondition(LaunchConfiguration("drivers"))
+            )
+          ]
         ),
 
         # LLP
-#       <!-- Connect and update environment variables if required -->
-#       <machine unless="$(eval arg('llp')=='local')" timeout="10"
-#                name="llp" address="$(arg llp)" user="astrobee" password="astrobee"
-#                env-loader="/opt/astrobee/env_wrapper.sh" default="true">
-#       </machine>
-        IncludeLaunchDescription(
-            get_launch_file("launch/robot/LLP.launch.py"),
-            launch_arguments={
-                "drivers": LaunchConfiguration("drivers"),  # Don't start driver nodes
-                "spurn"  : LaunchConfiguration("spurn"),    # Prevent node
-                "nodes"  : LaunchConfiguration("nodes"),    # Launch node group
-                "extra"  : LaunchConfiguration("extra"),    # Inject extra nodes
-                "debug"  : LaunchConfiguration("debug"),    # Debug a node set
-                "output" : LaunchConfiguration("output"), 
-                "gtloc"  : LaunchConfiguration("gtloc"),    # Use Ground Truth Localizer
-            }.items(),
-            condition=LaunchConfigurationNotEquals("llp", "disabled"),
+        #       <!-- Connect and update environment variables if required -->
+        #       <machine unless="$(eval arg('llp')=='local')" timeout="10"
+        #                name="llp" address="$(arg llp)" user="astrobee" password="astrobee"
+        #                env-loader="/opt/astrobee/env_wrapper.sh" default="true">
+        #       </machine>
+        GroupAction(
+            actions=[
+              PushRosNamespace(LaunchConfiguration('ns')),
+              IncludeLaunchDescription(
+                get_launch_file("launch/robot/LLP.launch.py"),
+                launch_arguments={
+                  "drivers": LaunchConfiguration("drivers"),  # Don't start driver nodes
+                  "spurn"  : LaunchConfiguration("spurn"),    # Prevent node
+                  "nodes"  : LaunchConfiguration("nodes"),    # Launch node group
+                  "extra"  : LaunchConfiguration("extra"),    # Inject extra nodes
+                  "debug"  : LaunchConfiguration("debug"),    # Debug a node set
+                  "output" : LaunchConfiguration("output"), 
+                  "gtloc"  : LaunchConfiguration("gtloc"),    # Use Ground Truth Localizer
+                }.items(),
+                condition=LaunchConfigurationNotEquals("llp", "disabled"),
+              )                      
+            ]
         ),
-
         # MLP
-#       <!-- Connect and update environment variables if required -->
-#       <machine unless="$(eval arg('mlp')=='local')" timeout="10"
-#                name="mlp" address="$(arg mlp)" user="astrobee" password="astrobee"
-#                env-loader="/opt/astrobee/env_wrapper.sh" default="true">
-#       </machine>
-        IncludeLaunchDescription(
-            get_launch_file("launch/robot/MLP.launch.py"),
-            launch_arguments={
+        #       <!-- Connect and update environment variables if required -->
+        #       <machine unless="$(eval arg('mlp')=='local')" timeout="10"
+        #                name="mlp" address="$(arg mlp)" user="astrobee" password="astrobee"
+        #                env-loader="/opt/astrobee/env_wrapper.sh" default="true">
+        #       </machine>
+        GroupAction(
+            actions=[
+              PushRosNamespace(LaunchConfiguration('ns')),
+              IncludeLaunchDescription(
+              get_launch_file("launch/robot/MLP.launch.py"),
+              launch_arguments={
                 "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes
                 "spurn"  : LaunchConfiguration("spurn"),   # Prevent node
                 "nodes"  : LaunchConfiguration("nodes"),   # Launch node group
                 "extra"  : LaunchConfiguration("extra"),   # Inject extra nodes
                 "debug"  : LaunchConfiguration("debug"),   # Debug a node set
                 "output" : LaunchConfiguration("output"), 
                 "gtloc"  : LaunchConfiguration("gtloc"),   # Use Ground Truth Localizer
-            }.items(),
-            condition=LaunchConfigurationNotEquals("llp", "disabled"),
-        ),
+              }.items(),
+              condition=LaunchConfigurationNotEquals("llp", "disabled"),
+              )
+            ]
+        )
+
     ])
 
 

astrobee/launch/controller/rviz.launch.py

@@ -33,6 +33,7 @@ def generate_launch_description():
             namespace="",
             executable="rviz2",
             name="rviz_node",
+            parameters=[{'use_sim_time': True}],  
             arguments=["-d", PathJoinSubstitution([get_package_share_directory("astrobee"),"resources/rviz",LaunchConfiguration("rviz_file")])],
             respawn=True,
         ),

astrobee/launch/robot/LLP.launch.py

@@ -59,12 +59,14 @@ def generate_launch_description():
                 package='ctl',
                 plugin='ctl::CtlComponent',
                 name='ctl',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ComposableNode(
                 package='fam',
                 plugin='fam::FamComponent',
                 name='fam',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
         ComposableNodeContainer(
@@ -193,7 +195,8 @@ def generate_launch_description():
                 package='light_flow',
                 plugin='light_flow::LightFlowComponent',
                 name='light_flow',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
     ])

astrobee/launch/robot/MLP.launch.py

@@ -59,12 +59,14 @@ def generate_launch_description():
                  package='localization_manager',
                  plugin='localization_manager::LocalizationManagerComponent',
                  name='localization_manager',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                 parameters=[{'use_sim_time': True}],
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ComposableNode(
                 package='ground_truth_localizer',
                 plugin='ground_truth_localizer::GroundTruthLocalizerComponent',
                 name='ground_truth_localizer',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],
+                extra_arguments=[{'use_intra_process_comms': False}]),
             # ComposableNode(
             #     package='image_sampler',
             #     plugin='image_sampler::ImageSampler',
@@ -85,7 +87,8 @@ def generate_launch_description():
                 package='localization_manager',
                 plugin='localization_manager::LocalizationManagerNodelet',
                 name='localization_manager',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],
+                extra_arguments=[{'use_intra_process_comms': False}]),
             # ComposableNode(
             #     package='image_sampler',
             #     plugin='image_sampler::ImageSampler',
@@ -226,25 +229,28 @@ def generate_launch_description():
                 package='mapper',
                 plugin='mapper::MapperComponent',
                 name='mapper',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
         ComposableNodeContainer(
         name='mlp_management',
         namespace='',
         package='rclcpp_components',
-        executable='component_container',
+        executable='component_container_mt',
         composable_node_descriptions=[
-            # ComposableNode(
-            #     package='executive',
-            #     plugin='executive::Executive',
-            #     name='executive',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
-            # ComposableNode(
-            #     package='access_control',
-            #     plugin='access_control::AccessControl',
-            #     name='access_control',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
+            ComposableNode(
+                package='executive',
+                plugin='executive::Executive',
+                name='executive',
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
+            ComposableNode(
+                package='access_control',
+                plugin='access_control::AccessControl',
+                name='access_control',
+                parameters=[{'use_sim_time': True}],
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
         ComposableNodeContainer(
@@ -291,11 +297,12 @@ def generate_launch_description():
         executable='component_container',
         condition=UnlessCondition(LaunchConfiguration("drivers")),
         composable_node_descriptions=[
-            # ComposableNode(
-            #     package='sys_monitor',
-            #     plugin='sys_monitor::SysMonitor',
-            #     name='sys_monitor',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
+             ComposableNode(
+                 package='sys_monitor',
+                 plugin='sys_monitor::SysMonitor',
+                 name='sys_monitor',
+                 parameters=[{'use_sim_time': True}],
+                 extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
         ComposableNodeContainer(
@@ -348,17 +355,20 @@ def generate_launch_description():
                 package='choreographer',
                 plugin='choreographer::ChoreographerComponent',
                 name='choreographer',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ComposableNode(
                 package='planner_trapezoidal',
                 plugin='planner_trapezoidal::PlannerTrapezoidalComponent',
                 name='planner_trapezoidal',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ComposableNode(
                 package='framestore',
                 plugin='mobility::FrameStore',
                 name='framestore',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
         ComposableNodeContainer(
@@ -370,7 +380,8 @@ def generate_launch_description():
             ComposableNode(
                 package='arm',
                 plugin='arm::ArmComponent',
-                name='arm'),
+                name='arm',
+                parameters=[{'use_sim_time': True}] ),
             ]
         ),
         ComposableNodeContainer(
@@ -382,8 +393,10 @@ def generate_launch_description():
             ComposableNode(
                 package='dock',
                 plugin='dock::DockComponent',
-                name='dock'),
-                #extra_arguments=[{'use_intra_process_comms': True}]),
+                name='dock',
+                parameters=[{'use_sim_time': True}]                
+                #extra_arguments=[{'use_intra_process_comms': False}]
+              ),
             ]
         ),
         ComposableNodeContainer(
@@ -395,8 +408,10 @@ def generate_launch_description():
             ComposableNode(
                 package='perch',
                 plugin='perch::PerchComponent',
-                name='perch'),
-                #extra_arguments=[{'use_intra_process_comms': True}]),
+                name='perch',
+                parameters=[{'use_sim_time': True}],                                
+                #extra_arguments=[{'use_intra_process_comms': True}]
+              ),
             ]
         ),
         ComposableNodeContainer(
@@ -423,7 +438,8 @@ def generate_launch_description():
                 package='states',
                 plugin='states::StatesComponent',
                 name='states',
-                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+                parameters=[{'use_sim_time': True}],                
+                extra_arguments=[{'use_intra_process_comms': False}]),
             ]
         ),
     ])