Add add_world_transform arg

husarion · Sep 17, 2024 · 511214e · 511214e
1 parent c626efa
commit 511214e
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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: