diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 4c271a8e4..34afcecac 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -31,12 +31,24 @@ def generate_launch_description(): + add_world_transform = LaunchConfiguration("add_world_transform") components_config_path = LaunchConfiguration("components_config_path") gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") namespace = LaunchConfiguration("namespace") robot_configuration = LaunchConfiguration("robot_configuration") robot_model = LaunchConfiguration("robot_model") use_ekf = LaunchConfiguration("use_ekf") + wheel_type = LaunchConfiguration("wheel_type") + + declare_add_world_transform_arg = DeclareLaunchArgument( + "add_world_transform", + default_value="False", + description=( + "Adds a world frame that connects the tf trees of individual robots (useful when running" + " multiple robots)." + ), + choices=["True", "true", "False", "false"], + ) declare_battery_config_path_arg = DeclareLaunchArgument( "battery_config_path", @@ -96,6 +108,7 @@ def generate_launch_description(): "namespace": namespace, "robot_model": robot_model, "use_sim": "True", + "wheel_type": wheel_type, }.items(), ) @@ -141,6 +154,7 @@ def generate_launch_description(): "namespace": namespace, "publish_robot_state": "False", "use_sim": "True", + "wheel_type": wheel_type, }.items(), ) @@ -191,10 +205,32 @@ def generate_launch_description(): emulate_tty=True, ) + child_tf = PythonExpression(["'", namespace, "' + '/odom' if '", namespace, "' else 'odom'"]) + + world_transform = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_publisher", + arguments=[ + LaunchConfiguration("x"), + LaunchConfiguration("y"), + LaunchConfiguration("z"), + LaunchConfiguration("roll"), + LaunchConfiguration("pitch"), + LaunchConfiguration("yaw"), + "world", + child_tf, + ], + namespace=namespace, + emulate_tty=True, + condition=IfCondition(add_world_transform), + ) + return LaunchDescription( [ declare_robot_configuration_arg, DeclareRobotArgs(robot_configuration), + declare_add_world_transform_arg, declare_battery_config_path_arg, declare_components_config_path_arg, declare_gz_bridge_config_path_arg, @@ -207,5 +243,6 @@ def generate_launch_description(): ekf_launch, simulate_components, gz_bridge, + world_transform, ] ) diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/panther_gazebo/launch/spawn_robot.launch.py index 5a54c3694..173f3e60a 100644 --- a/panther_gazebo/launch/spawn_robot.launch.py +++ b/panther_gazebo/launch/spawn_robot.launch.py @@ -39,6 +39,7 @@ def generate_launch_description(): roll = LaunchConfiguration("roll") pitch = LaunchConfiguration("pitch") yaw = LaunchConfiguration("yaw") + wheel_type = LaunchConfiguration("wheel_type") declare_robot_configuration_arg = DeclareLaunchArgument( "robot_configuration", @@ -67,6 +68,7 @@ def generate_launch_description(): "add_wheel_joints": add_wheel_joints, "namespace": namespace, "robot_model": robot_model, + "wheel_type": wheel_type, "use_sim": "True", }.items(), ) diff --git a/panther_utils/panther_utils/arguments.py b/panther_utils/panther_utils/arguments.py index ad9a7f2e7..05eb46223 100644 --- a/panther_utils/panther_utils/arguments.py +++ b/panther_utils/panther_utils/arguments.py @@ -34,10 +34,10 @@ class DeclareRobotArgs(Action): """Retrieves and validate the robot configuration from the YAML data.""" - def __init__(self, path: LaunchConfiguration, **kwargs) -> None: + def __init__(self, robot_configuration_path: LaunchConfiguration, **kwargs) -> None: """Create a DeclareRobotArgs action.""" super().__init__(**kwargs) - self.__path = normalize_to_list_of_substitutions(path) + self.__path = normalize_to_list_of_substitutions(robot_configuration_path) @property def path(self) -> Text: