more work..

Author: mdurrani808

src/simulation/launch/bridge.launch.py

@@ -2,13 +2,38 @@
 from launch_ros.actions import Node
 
 def generate_launch_description():
+    # All bridge nodes use simulation time since they only run in simulation
     return LaunchDescription([
+        Node(
+            package='ros_gz_bridge',
+            executable='parameter_bridge',
+            name='imu_bridge',
+            output='screen',
+            parameters=[{'use_sim_time': True}],
+            arguments=[
+                '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
+            ],
+        ),
+
+        Node(
+            package='ros_gz_bridge',
+            executable='parameter_bridge',
+            name='gps_bridge',
+            output='screen',
+            parameters=[{'use_sim_time': True}],
+            arguments=[
+                '/gps/fix@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat',
+            ],
+        ),
+
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
             name='depth_camera_bridge',
             output='screen',
+            parameters=[{'use_sim_time': True}],
             arguments=[
+                '/depth_camera@sensor_msgs/msg/Image@gz.msgs.Image',
                 '/depth_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
             ],
         ),
@@ -18,6 +43,7 @@ def generate_launch_description():
             executable='parameter_bridge',
             name='clock_bridge',
             output='screen',
+            parameters=[{'use_sim_time': True}],
             arguments=[
                 '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
             ]
@@ -28,6 +54,7 @@ def generate_launch_description():
             executable='parameter_bridge',
             name='ground_truth_odom_bridge',
             output='screen',
+            parameters=[{'use_sim_time': True}],
             arguments=[
                 '/odom/ground_truth@nav_msgs/msg/Odometry@gz.msgs.Odometry',
             ]

src/simulation/launch/ground_truth_tf.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
             output='screen',
             parameters=[{
                 'child_frame_id': LaunchConfiguration('child_frame_id'),
-                'use_sim_time': True
+                'use_sim_time': True  # Always true since this only runs in simulation
             }]
         ),
     ])

src/simulation/launch/sim_bringup.launch.py

@@ -1,6 +1,7 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
@@ -11,10 +12,10 @@
         description='Gazebo world file to load'
     ),
     DeclareLaunchArgument(
-        'use_sim_time',
+        'publish_ground_truth_tf',
         default_value='true',
         choices=['true', 'false'],
-        description='Use simulation time from Gazebo'
+        description='Publish ground truth odom -> base_footprint transform'
     ),
 ]
 
@@ -26,7 +27,6 @@ def generate_launch_description():
     gazebo = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
-            ('use_sim_time', LaunchConfiguration('use_sim_time')),
             ('world', LaunchConfiguration('world'))
         ]
     )
@@ -36,7 +36,14 @@ def generate_launch_description():
         PythonLaunchDescriptionSource([bridge_launch])
     )
 
+    ground_truth_tf_launch = PathJoinSubstitution([pkg_sim, 'launch', 'ground_truth_tf.launch.py'])
+    ground_truth_tf = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ground_truth_tf_launch]),
+        condition=IfCondition(LaunchConfiguration('publish_ground_truth_tf'))
+    )
+
     ld = LaunchDescription(ARGUMENTS)
     ld.add_action(gazebo)
     ld.add_action(bridge)
+    ld.add_action(ground_truth_tf)
     return ld

src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py

@@ -67,6 +67,32 @@ def generate_launch_description():
         description="Gazebo world file to load (only used when use_sim:=true)",
     )
 
+    world_name_arg = DeclareLaunchArgument(
+        "world_name",
+        default_value="",
+        description="Name of the world inside Gazebo (optional)",
+    )
+
+    rqt_arg = DeclareLaunchArgument(
+        "rqt",
+        default_value="false",
+        choices=["true", "false"],
+        description="Start rqt_image_view for camera topics",
+    )
+
+    image_topic_arg = DeclareLaunchArgument(
+        "image_topic",
+        default_value="/depth_camera",
+        description="Image topic to view in rqt_image_view",
+    )
+
+    publish_ground_truth_tf_arg = DeclareLaunchArgument(
+        "publish_ground_truth_tf",
+        default_value="true",
+        choices=["true", "false"],
+        description="Publish ground truth TF in simulation",
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     runtime_config_package = LaunchConfiguration("runtime_config_package")
     description_package = LaunchConfiguration("description_package")
@@ -76,6 +102,10 @@ def generate_launch_description():
     start_rviz = LaunchConfiguration("start_rviz")
     rviz_file = LaunchConfiguration("rviz_file")
     world = LaunchConfiguration("world")
+    world_name = LaunchConfiguration("world_name")
+    rqt = LaunchConfiguration("rqt")
+    image_topic = LaunchConfiguration("image_topic")
+    publish_ground_truth_tf = LaunchConfiguration("publish_ground_truth_tf")
 
     robot_controllers_path = PathJoinSubstitution(
         [FindPackageShare(runtime_config_package), "config", "athena_drive_controllers.yaml"]
@@ -91,7 +121,7 @@ def generate_launch_description():
         ]),
         launch_arguments={
             "world": world,
-            "use_sim_time": "true",
+            "publish_ground_truth_tf": publish_ground_truth_tf,
         }.items(),
         condition=IfCondition(use_sim),
     )
@@ -129,17 +159,18 @@ def generate_launch_description():
         }.items(),
     )
 
-    hardware = IncludeLaunchDescription(
+    spawn_robot = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
             PathJoinSubstitution([
                 FindPackageShare(runtime_config_package),
                 "launch",
-                "hardware.launch.py"
+                "spawn.launch.py"
             ])
         ]),
         launch_arguments={
             "use_sim": use_sim,
             "robot_name": "rover",
+            "world_name": world_name,
             "spawn_x": "0.0",
             "spawn_y": "0.0",
             "spawn_z": "3.0",
@@ -174,6 +205,8 @@ def generate_launch_description():
             "description_package": description_package,
             "rviz_file": rviz_file,
             "use_sim": use_sim,
+            "rqt": rqt,
+            "image_topic": image_topic,
         }.items(),
         condition=IfCondition(start_rviz),
     )
@@ -189,11 +222,15 @@ def generate_launch_description():
         robot_controller_arg,
         start_rviz_arg,
         world_arg,
+        world_name_arg,
+        rqt_arg,
+        image_topic_arg,
+        publish_ground_truth_tf_arg,
         # Launch files and nodes
         sim_bringup,
         robot_description,
         teleop,
-        hardware,
+        spawn_robot,
         controllers,
         visualization,
     ])

src/subsystems/drive/drive_bringup/launch/controllers.launch.py

@@ -34,38 +34,44 @@ def controller_spawning_logic(context, *args, **kwargs):
         remappings=[
             ("~/robot_description", "/robot_description"),
         ],
-        condition=UnlessCondition(use_sim),
-    )
-
-    cmd_vel_relay = Node(
-        package="topic_tools",
-        executable="relay",
-        name="cmd_vel_to_ackermann_relay",
-        arguments=[
-            "/cmd_vel",
-            "/ackermann_steering_controller/reference",
-            "--ros-args",
-            "--log-level",
-            "fatal",
-        ],
-        parameters=[{"use_sim_time": use_sim}],
-        output="log",
+        #condition=UnlessCondition(use_sim),
     )
 
-    joy_relay = Node(
-        package="topic_tools",
-        executable="relay",
-        name="joy_to_single_ackermann_relay",
-        arguments=[
-            "/joy",
-            "/single_ackermann_controller/reference",
-            "--ros-args",
-            "--log-level",
-            "fatal",
-        ],
-        parameters=[{"use_sim_time": use_sim}],
-        output="log",
-    )
+    # Relay nodes: route cmd_vel to the correct controller's reference topic
+    relay_nodes = []
+
+    if robot_controller == "ackermann_steering_controller":
+        # For ackermann_steering_controller: relay cmd_vel to its reference topic
+        relay_nodes.append(Node(
+            package="topic_tools",
+            executable="relay",
+            name="cmd_vel_relay",
+            arguments=[
+                "/cmd_vel",
+                "/ackermann_steering_controller/reference",
+                "--ros-args",
+                "--log-level",
+                "fatal",
+            ],
+            parameters=[{"use_sim_time": use_sim}],
+            output="log",
+        ))
+    elif robot_controller == "single_ackermann_controller":
+        # For single_ackermann_controller: relay cmd_vel to its reference topic
+        relay_nodes.append(Node(
+            package="topic_tools",
+            executable="relay",
+            name="cmd_vel_relay",
+            arguments=[
+                "/cmd_vel",
+                "/single_ackermann_controller/reference",
+                "--ros-args",
+                "--log-level",
+                "fatal",
+            ],
+            parameters=[{"use_sim_time": use_sim}],
+            output="log",
+        ))
 
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
@@ -155,8 +161,7 @@ def controller_spawning_logic(context, *args, **kwargs):
 
     return [
         control_node,
-        cmd_vel_relay,
-        joy_relay,
+        *relay_nodes,
         delayed_controllers_real,
         delayed_controllers_sim,
     ]

src/subsystems/drive/drive_bringup/launch/rviz.launch.py

@@ -1,5 +1,6 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, TimerAction
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -28,12 +29,25 @@ def generate_launch_description():
             default_value="2.0",
             description="Delay in seconds before starting RViz.",
         ),
+        DeclareLaunchArgument(
+            "rqt",
+            default_value="false",
+            choices=["true", "false"],
+            description="Start rqt_image_view for camera topics.",
+        ),
+        DeclareLaunchArgument(
+            "image_topic",
+            default_value="/depth_camera",
+            description="Image topic to view in rqt_image_view.",
+        ),
     ]
 
     description_package = LaunchConfiguration("description_package")
     rviz_file = LaunchConfiguration("rviz_file")
     use_sim = LaunchConfiguration("use_sim")
     startup_delay = LaunchConfiguration("startup_delay")
+    rqt = LaunchConfiguration("rqt")
+    image_topic = LaunchConfiguration("image_topic")
 
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare(description_package), "rviz", rviz_file]
@@ -54,4 +68,13 @@ def generate_launch_description():
         actions=[rviz_node],
     )
 
-    return LaunchDescription(declared_arguments + [delayed_rviz_launch])
+    rqt_image_view_node = Node(
+        package="rqt_image_view",
+        executable="rqt_image_view",
+        name="rqt_image_view",
+        arguments=[image_topic],
+        parameters=[{"use_sim_time": use_sim}],
+        condition=IfCondition(rqt),
+    )
+
+    return LaunchDescription(declared_arguments + [delayed_rviz_launch, rqt_image_view_node])

…/drive_bringup/launch/hardware.launch.py …ive/drive_bringup/launch/spawn.launch.pysrc/subsystems/drive/drive_bringup/launch/hardware.launch.py renamed to src/subsystems/drive/drive_bringup/launch/spawn.launch.py src/subsystems/drive/drive_bringup/launch/hardware.launch.py renamed to src/subsystems/drive/drive_bringup/launch/spawn.launch.py

@@ -19,6 +19,12 @@ def generate_launch_description():
         description="Name of the robot in Gazebo.",
     )
 
+    world_name_arg = DeclareLaunchArgument(
+        "world_name",
+        default_value="",
+        description="Name of the world inside Gazebo (optional).",
+    )
+
     spawn_args = [
         DeclareLaunchArgument("spawn_x", default_value="0.0", description="Spawn X"),
         DeclareLaunchArgument("spawn_y", default_value="0.0", description="Spawn Y"),
@@ -28,6 +34,7 @@ def generate_launch_description():
 
     use_sim = LaunchConfiguration("use_sim")
     robot_name = LaunchConfiguration("robot_name")
+    world_name = LaunchConfiguration("world_name")
     spawn_x = LaunchConfiguration("spawn_x")
     spawn_y = LaunchConfiguration("spawn_y")
     spawn_z = LaunchConfiguration("spawn_z")
@@ -39,6 +46,7 @@ def generate_launch_description():
         executable="create",
         arguments=[
             "-name", robot_name,
+            "-world", world_name,
             "-x", spawn_x,
             "-y", spawn_y,
             "-z", spawn_z,
@@ -53,6 +61,7 @@ def generate_launch_description():
         [
             use_sim_arg,
             robot_name_arg,
+            world_name_arg,
             *spawn_args,
             spawn_robot,
         ]