Merge branch 'humble-devel' into testing

Author: Apolo151

CustomRobots/CMakeLists.txt

@@ -97,6 +97,7 @@ install(
     car_junction/models
     car_junction/worlds
     car_junction/launch
+    car_junction/scripts
     # AMAZON WAREHOUSE ROBOT
     amazon_robot/launch
     amazon_robot/models

CustomRobots/car_junction/launch/car_junction.launch.py

@@ -0,0 +1,115 @@
+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="custom_robots").find("custom_robots")
+
+    # Set the path to the world file
+    world_file_name = "road_junction.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"
+    ] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}"
+
+    start_ros_gazebo_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
+            "/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist", 
+            "/odom@nav_msgs/msg/Odometry]gz.msgs.Odometry",
+            "/waymo/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked",
+            "/waymo/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo",
+        ],
+        parameters=[{'use_sim_time': True}],
+        output="screen"
+    ),
+    
+    start_ros_gazebo_image_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        arguments=["/waymo/camera_front@sensor_msgs/msg/Image[gz.msgs.Image"],
+        output="screen",
+    )
+
+    ########### 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)
+    ld.add_action(start_ros_gazebo_bridge)
+    ld.add_action(start_ros_gazebo_image_bridge)
+
+    return ld