Fixed Launch Bug

.vscode/keybindings.json

@@ -1,7 +1,22 @@
 [
     {
-        "key": "ctrl+shift+alt+g",
+        "key": "ctrl+shift+alt+2",
         "command": "workbench.action.tasks.runTask",
-        "args": "Start Gazebo"
+        "args": "Day 2"
+    },
+    {
+        "key": "ctrl+shift+alt+3",
+        "command": "workbench.action.tasks.runTask",
+        "args": "Day 3"
+    },
+    {
+        "key": "ctrl+shift+alt+4",
+        "command": "workbench.action.tasks.runTask",
+        "args": "Day 4"
+    },
+    {
+        "key": "ctrl+shift+alt+5",
+        "command": "workbench.action.tasks.runTask",
+        "args": "Day 5"
     }
 ]

.vscode/scripts/day2.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+bash .vscode/scripts/build.sh
+
+source install/setup.bash
+ign gazebo shapes.sdf

.vscode/scripts/day3.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+bash .vscode/scripts/build.sh
+
+source install/setup.bash
+ros2 launch start_creating_robots gazebo.launch.py
+

.vscode/scripts/day4.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+bash .vscode/scripts/build.sh
+
+source install/setup.bash
+ros2 launch start_creating_robots mapping.launch.py

.vscode/scripts/day5.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+bash .vscode/scripts/build.sh
+
+source install/setup.bash
+ros2 launch start_creating_robots navigation.launch.py

.vscode/tasks.json

@@ -6,7 +6,7 @@
             "label": "build",
             "detail": "Build workspace (default)",
             "type": "shell",
-            "command": "bash build.sh",
+            "command": "bash .vscode/scripts/build.sh",
             "group": {
                 "kind": "build",
                 "isDefault": true
@@ -22,9 +22,28 @@
             }
         },
         {
-            "label": "Start Gazebo",
+            "label": "Day 2",
+            "detail": "Test that Gazebo works!",
             "type": "shell",
-            "command": "ign gazebo shapes.sdf"
+            "command": "bash .vscode/scripts/day2.sh"
+        },
+        {
+            "label": "Day 3",
+            "detail": "Teleoperate a robot",
+            "type": "shell",
+            "command": "bash .vscode/scripts/day3.sh"
+        },
+        {
+            "label": "Day 4",
+            "detail": "Create your first map",
+            "type": "shell",
+            "command": "bash .vscode/scripts/day4.sh"
+        },
+        {
+            "label": "Day 5",
+            "detail": "Let your robot drive itself",
+            "type": "shell",
+            "command": "bash .vscode/scripts/day5.sh"
         }
     ]
 }

src/start_creating_robots/launch/gazebo.launch.py

@@ -1,81 +1,72 @@
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.actions import IncludeLaunchDescription, ExecuteProcess, DeclareLaunchArgument
 from launch.launch_description_sources import get_launch_description_from_python_launch_file
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 from os.path import join
+from launch.substitutions import LaunchConfiguration, Command
 
 import xacro
 
 
 def generate_launch_description():
 
+    # This allows us to have the with_sensors as an argument on the command line
+    with_sensors_arg =  DeclareLaunchArgument(
+        'with_sensors', default_value="false"
+    )
+    # This allows us to use the with_sensors variable in substitutions in this launch description.
+    with_sensors = LaunchConfiguration('with_sensors', default="false")
+
+
     models_path = join(get_package_share_directory("start_creating_robots"), "worlds_and_models")
+
+    # Start a simulation with the cafe world
     cafe_world_uri = join(models_path,"cafe.sdf")
     path = join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py")
+
+    gazebo_sim = IncludeLaunchDescription(path,
+                                          launch_arguments=[("gz_args", '-r ' + cafe_world_uri)])
+
+    # Create a robot in the world.
+    # Steps: 
+    # 1. Process a file using the xacro tool to get an xml file containing the robot description.
+    # 2. Publish this robot description using a ros topic so all nodes can know about the joints of the robot. 
+    # 3. Spawn a simulated robot in the gazebo simulation using the published robot description topic. 
 
+    # Step 1. Process robot file. 
     robot_file = join(models_path, "krytn","krytn.urdf.xacro")
-    robot_description_config = xacro.process_file(robot_file)
-    robot_description = {'robot_description': robot_description_config.toxml()}
-
-    # Robot state publisher
+    robot_xml = Command(["xacro ",robot_file," with_sensors:=",with_sensors])
+
+    #Step 2. Publish robot file to ros topic /robot_description
     robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='both',
-        parameters=[robot_description],
+        parameters=[{'robot_description':robot_xml}],
     )
 
-    gazebo_sim = IncludeLaunchDescription(path,
-                                          launch_arguments=[("gz_args", '-r ' + cafe_world_uri)])
-
-
+    # Step 3. Spawn a robot in gazebo by listening to the published topic.
     robot = ExecuteProcess(
         cmd=["ros2", "run", "ros_gz_sim", "create", "-topic", "robot_description", "-z", "0.5"],
         name="spawn robot",
         output="both"
     )
 
-
-    # Gazebo Bridge: This allows communication from the Gazebo world to the ROS system.
-
+    # Gazebo Bridge: This allows ROS to send messages to drive the robot in simulation. 
     bridge = Node(
         package='ros_gz_bridge',
         executable='parameter_bridge',
-        arguments=['/model/krytn/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
-                   '/model/krytn/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
-                   '/model/krytn/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
-                   '/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
-                   '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
-                   '/realsense/image@sensor_msgs/msg/Image[gz.msgs.Image',
-                   '/realsense/depth@sensor_msgs/msg/Image[gz.msgs.Image',
-                   '/realsense/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
-                   '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
-                   '/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model'],
+        arguments=['/model/krytn/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist'],
         output='screen',
-        remappings=[('/model/krytn/odometry','/odom'),
-                    ('/model/krytn/tf','/tf'),
-                    ('/model/krytn/cmd_vel','/cmd_vel')]
+        remappings=[('/model/krytn/cmd_vel','/cmd_vel')]
     )
 
-    # Gazebo fortress has a bug that won't respect our frame_id tags. So we have to publish a transform 
-    depth_cam_link_tf = Node(package='tf2_ros',
-                     executable='static_transform_publisher',
-                     name='depthCamLinkTF',
-                     output='log',
-                     arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'realsense_d435', 'krytn/base_footprint/realsense_d435'])
-
-    krytn_base_fp_link_tf = Node(package='tf2_ros',
-                     executable='static_transform_publisher',
-                     name='base_fp_linkTF',
-                     output='log',
-                     arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0',  'krytn/base_footprint', 'base_footprint'])
-
-
+    # A gui tool for easy tele-operation.
     robot_steering = Node(
         package="rqt_robot_steering",
         executable="rqt_robot_steering",
     )
 
-    return LaunchDescription([gazebo_sim, bridge, robot, robot_steering, robot_state_publisher, depth_cam_link_tf, krytn_base_fp_link_tf])
+    return LaunchDescription([gazebo_sim, bridge, robot, robot_steering, robot_state_publisher, with_sensors_arg])

src/start_creating_robots/launch/mapping.launch.py

@@ -1,28 +1,68 @@
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
 from ament_index_python.packages import get_package_share_directory
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import PathJoinSubstitution, TextSubstitution
-from launch.substitutions import LaunchConfiguration
 
-import os
+from os.path import join
 
 def generate_launch_description():
-    # ... your existing nodes ...
+    # This allows us to have the with_sensors as an argument on the command line
+    rviz_config_arg =  DeclareLaunchArgument(
+        'rviz_config', default_value="mapping.yaml"
+    )
+    # This allows us to use the with_sensors variable in substitutions in this launch description.
+    rviz_config = LaunchConfiguration('rviz_config', default="vis.rviz")
+
+    base_path = get_package_share_directory("start_creating_robots")
+
+    # We will include everything from the gazebo launch file, making sure that sensors are now enabled however.
+    gazebo = IncludeLaunchDescription(join(base_path, "launch","gazebo.launch.py"), 
+                                      launch_arguments=[("with_sensors","true")])
+
+    # Extended Gazebo Bridge: To do mapping requires alot more info from the simulation. We need sensor data and estimates of the robot joint positions.
+    extended_bridge = Node( package='ros_gz_bridge', name="extended_gazebo_bridge", executable='parameter_bridge', arguments=['/model/krytn/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
+                   '/model/krytn/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
+
+                   '/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
+                   '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
+
+                   '/realsense/image@sensor_msgs/msg/Image[gz.msgs.Image',
+                   '/realsense/depth@sensor_msgs/msg/Image[gz.msgs.Image',
+                   '/realsense/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
+
+                   '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+
+                   '/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model'], output='screen', remappings=[('/model/krytn/odometry','/odom'),
+                    ('/model/krytn/tf','/tf')]
+    )
+
+    # Gazebo fortress has a bug that won't respect our frame_id tags. So we have to publish a transform 
+    depth_cam_link_tf = Node(package='tf2_ros', executable='static_transform_publisher', name='depthCamLinkTF', output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'realsense_d435', 'krytn/base_footprint/realsense_d435'])
+
+    krytn_base_fp_link_tf = Node(package='tf2_ros', executable='static_transform_publisher', name='base_fp_linkTF', output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0',  'krytn/base_footprint', 'base_footprint'])
 
     # SLAM Toolbox for mapping
-    slam_toolbox = Node(
-        package='slam_toolbox',
-        executable='async_slam_toolbox_node',
-        parameters=[
+    slam_toolbox = Node( package='slam_toolbox', executable='async_slam_toolbox_node', parameters=[
             get_package_share_directory('start_creating_robots') + '/config/mapping.yaml'
-        ],
-        output='screen'
+        ], output='screen'
+    )
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=[
+            '-d',
+            PathJoinSubstitution([base_path, 'config', rviz_config])
+        ]
     )
 
 
-    return LaunchDescription([
-        slam_toolbox
-    ])
+    return LaunchDescription([gazebo,
+                            extended_bridge, 
+                            depth_cam_link_tf, 
+                            krytn_base_fp_link_tf,
+                            slam_toolbox, 
+                            rviz, 
+                            rviz_config_arg])

src/start_creating_robots/launch/navigation.launch.py

@@ -9,10 +9,16 @@
 from ament_index_python.packages import get_package_share_directory
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-
+from launch.actions import IncludeLaunchDescription
+from os.path import join
 
 def generate_launch_description():
 
+    base_path = get_package_share_directory("start_creating_robots")
+
+    # We will include everything from the mapping launch file, making sure that sensors are now enabled and setting up RVIZ for navigation.
+    gazebo_and_mapping = IncludeLaunchDescription(join(base_path, "launch","mapping.launch.py"),
+                                      launch_arguments=[("with_sensors","true"), ("rviz_config","navigation.yaml")])
 
     # Nav2 bringup for navigation
     navigation = IncludeLaunchDescription(
@@ -25,5 +31,5 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        navigation
+        gazebo_and_mapping, navigation
     ])

src/start_creating_robots/setup.py

@@ -16,6 +16,7 @@
         (join('share', package_name, 'launch'), glob('launch/*launch.py')),
 
         (join('share', package_name, 'config'), glob('config/*.yaml')),
+        (join('share', package_name, 'config'), glob('config/*.rviz')),
 
         (join('share', package_name, 'worlds_and_models'), glob('worlds_and_models/*.*')),