diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py
index 78a3382e..f3dc0988 100644
--- a/urc_bringup/launch/sim.launch.py
+++ b/urc_bringup/launch/sim.launch.py
@@ -4,7 +4,7 @@
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, PythonExpression
from launch_ros.descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
@@ -47,14 +47,66 @@ def generate_launch_description():
description="Spawn rocker effort controller (Option B)",
)
- autonomy_arg = DeclareLaunchArgument(
- "autonomy",
- default_value="false",
- description="Launch autonomy nodes",
- )
-
- world_filename = LaunchConfiguration("world")
- walli_xacro_config = LaunchConfiguration("walli_xacro")
+ autonomy_arg = DeclareLaunchArgument(
+ "autonomy",
+ default_value="false",
+ description="Launch autonomy nodes",
+ )
+
+ add_cube_arg = DeclareLaunchArgument(
+ "add_cube",
+ default_value="false",
+ description="Spawn a large cube obstacle in front of the rover",
+ )
+
+ cube_x_arg = DeclareLaunchArgument(
+ "cube_x",
+ default_value="8.0",
+ description="X position for front cube obstacle (used when add_cube is true)",
+ )
+
+ cube_y_arg = DeclareLaunchArgument(
+ "cube_y",
+ default_value="0.0",
+ description="Y position for front cube obstacle (used when add_cube is true)",
+ )
+
+ add_incline_arg = DeclareLaunchArgument(
+ "add_incline",
+ default_value="false",
+ description="Spawn an inclined plane obstacle",
+ )
+
+ incline_x_arg = DeclareLaunchArgument(
+ "incline_x",
+ default_value="2.0",
+ description="X position for incline obstacle (used when add_incline is true)",
+ )
+
+ incline_y_arg = DeclareLaunchArgument(
+ "incline_y",
+ default_value="0.0",
+ description="Y position for incline obstacle (used when add_incline is true)",
+ )
+
+ incline_slope_arg = DeclareLaunchArgument(
+ "incline_slope",
+ default_value="15.0",
+ description="Incline slope in degrees (used when add_incline is true)",
+ )
+
+ world_filename = LaunchConfiguration("world")
+ walli_xacro_config = LaunchConfiguration("walli_xacro")
+ cube_x = LaunchConfiguration("cube_x")
+ cube_y = LaunchConfiguration("cube_y")
+ incline_x = LaunchConfiguration("incline_x")
+ incline_y = LaunchConfiguration("incline_y")
+ incline_slope = LaunchConfiguration("incline_slope")
+ cube_sdf_path = os.path.join(path_urc_hw_description, "world", "obstacles", "large_cube.sdf")
+ incline_sdf_path = os.path.join(path_urc_hw_description, "world", "obstacles", "incline_plane.sdf")
+ incline_pitch_rad = PythonExpression(
+ ["-float(", incline_slope, ") * 3.141592653589793 / 180.0"]
+ )
world_path = PathJoinSubstitution([path_urc_hw_description, "world", world_filename])
@@ -160,10 +212,10 @@ def generate_launch_description():
output="screen",
)
- spawn = Node(
- package="ros_gz_sim",
- executable="create",
- output="screen",
+ spawn = Node(
+ package="ros_gz_sim",
+ executable="create",
+ output="screen",
arguments=[
"-name", "walli",
"-x", "0",
@@ -172,10 +224,44 @@ def generate_launch_description():
"-R", "0",
"-P", "0",
"-Y", "0",
- "-topic", "robot_description",
- ],
- )
-
+ "-topic", "robot_description",
+ ],
+ )
+
+ spawn_cube = Node(
+ package="ros_gz_sim",
+ executable="create",
+ output="screen",
+ condition=IfCondition(LaunchConfiguration("add_cube")),
+ arguments=[
+ "-name", "cube_obstacle",
+ "-x", cube_x,
+ "-y", cube_y,
+ "-z", "1.0",
+ "-R", "0.0",
+ "-P", "0.0",
+ "-Y", "0.0",
+ "-file", cube_sdf_path,
+ ],
+ )
+
+ spawn_incline = Node(
+ package="ros_gz_sim",
+ executable="create",
+ output="screen",
+ condition=IfCondition(LaunchConfiguration("add_incline")),
+ arguments=[
+ "-name", "incline_obstacle",
+ "-x", incline_x,
+ "-y", incline_y,
+ "-z", "0.3",
+ "-R", "0.0",
+ "-P", incline_pitch_rad,
+ "-Y", "0.0",
+ "-file", incline_sdf_path,
+ ],
+ )
+
# --- Controller Spawners ---
load_joint_state_broadcaster = Node(
package="controller_manager",
@@ -240,19 +326,28 @@ def generate_launch_description():
[
sim_world_arg,
walli_xacro,
- bridge_yaml,
- spawn_rocker_effort,
- autonomy_arg,
- gz_sim,
- bridge,
- robot_state_publisher_node,
+ bridge_yaml,
+ spawn_rocker_effort,
+ autonomy_arg,
+ add_cube_arg,
+ cube_x_arg,
+ cube_y_arg,
+ add_incline_arg,
+ incline_x_arg,
+ incline_y_arg,
+ incline_slope_arg,
+ gz_sim,
+ bridge,
+ robot_state_publisher_node,
covariances_on_imu,
covariances_on_gps,
launch_ekf,
launch_autonomy,
- rocker_tf_broadcaster,
- rocker_effort_pid_node,
- spawn,
+ rocker_tf_broadcaster,
+ rocker_effort_pid_node,
+ spawn,
+ spawn_cube,
+ spawn_incline,
# After robot spawn, start controller spawners on a fixed schedule
RegisterEventHandler(
diff --git a/urc_hw_description/world/obstacles/incline_plane.sdf b/urc_hw_description/world/obstacles/incline_plane.sdf
new file mode 100644
index 00000000..42e5145e
--- /dev/null
+++ b/urc_hw_description/world/obstacles/incline_plane.sdf
@@ -0,0 +1,28 @@
+
+
+
+ true
+ 0 0 0 0 0 0
+
+
+
+
+ 2.0 2.0 0.2
+
+
+
+
+
+
+ 2.0 2.0 0.2
+
+
+
+ 0.2 0.3 0.8 1.0
+ 0.3 0.4 0.9 1.0
+ 0.1 0.1 0.1 1.0
+
+
+
+
+
diff --git a/urc_hw_description/world/obstacles/large_cube.sdf b/urc_hw_description/world/obstacles/large_cube.sdf
new file mode 100644
index 00000000..06b983b5
--- /dev/null
+++ b/urc_hw_description/world/obstacles/large_cube.sdf
@@ -0,0 +1,28 @@
+
+
+
+ true
+ 0 0 0 0 0 0
+
+
+
+
+ 2.0 2.0 2.0
+
+
+
+
+
+
+ 2.0 2.0 2.0
+
+
+
+ 0.8 0.2 0.2 1.0
+ 0.9 0.3 0.3 1.0
+ 0.1 0.1 0.1 1.0
+
+
+
+
+