create overwrite_robot_description.launch

panther_description/launch/overwrite_robot_description.launch.py

@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 ros2_control Development Team
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    Command,
+    EnvironmentVariable,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    add_wheel_joints = LaunchConfiguration("add_wheel_joints")
+    declared_add_wheel_joints_arg = DeclareLaunchArgument(
+        "add_wheel_joints",
+        default_value="True",
+        description="Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    battery_config_path = LaunchConfiguration("battery_config_path")
+    declare_battery_config_path_arg = DeclareLaunchArgument(
+        "battery_config_path",
+        description=(
+            "Path to the Ignition LinearBatteryPlugin configuration file. "
+            "This configuration is intended for use in simulations only."
+        ),
+        default_value="",
+    )
+
+    components_config_path = LaunchConfiguration("components_config_path")
+    declare_components_config_path_arg = DeclareLaunchArgument(
+        "components_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_description"), "config", "components.yaml"]
+        ),
+        description=(
+            "Additional components configuration file. Components described in this file "
+            "are dynamically included in Panther's urdf."
+            "Panther options are described here "
+            "https://husarion.com/manuals/panther/panther-options/"
+        ),
+    )
+
+    wheel_type = LaunchConfiguration("wheel_type")
+    controller_config_path = LaunchConfiguration("controller_config_path")
+    declare_controller_config_path_arg = DeclareLaunchArgument(
+        "controller_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("husarion_ugv_controller"),
+                "config",
+                PythonExpression(["'", wheel_type, "_controller.yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to controller configuration file. By default, it is located in"
+            " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify"
+            " the path to your custom controller configuration file here. "
+        ),
+    )
+
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used.",
+        choices=["True", "true", "False", "false"],
+    )
+
+    wheel_config_path = LaunchConfiguration("wheel_config_path")
+    declare_wheel_config_path_arg = DeclareLaunchArgument(
+        "wheel_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("panther_description"),
+                "config",
+                PythonExpression(["'", wheel_type, ".yaml'"]),
+            ]
+        ),
+        description=(
+            "Path to wheel configuration file. By default, it is located in "
+            "'panther_description/config/{wheel_type}.yaml'. You can also specify the path "
+            "to your custom wheel configuration file here. "
+        ),
+    )
+
+    declare_wheel_type_arg = DeclareLaunchArgument(
+        "wheel_type",
+        default_value="WH01",
+        description=(
+            "Specify the wheel type. If the selected wheel type is not 'custom', "
+            "the 'wheel_config_path' and 'controller_config_path' arguments will be "
+            "automatically adjusted and can be omitted."
+        ),
+        choices=["WH01", "WH02", "WH04", "custom"],
+    )
+
+    # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"]
+            ),
+            " use_sim:=",
+            use_sim,
+            " wheel_config_file:=",
+            wheel_config_path,
+            " controller_config_file:=",
+            controller_config_path,
+            " battery_config_file:=",
+            battery_config_path,
+            " imu_xyz:=",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
+            " imu_rpy:=",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
+            " namespace:=",
+            namespace,
+            " components_config_path:=",
+            components_config_path,
+        ]
+    )
+
+    set_robot_description = ExecuteProcess(
+        cmd=["ros2", "param", "set", "/panther/robot_state_publisher", "robot_description", robot_description_content],
+        output="screen",
+    )
+
+    actions = [
+        declared_add_wheel_joints_arg,
+        declare_battery_config_path_arg,
+        declare_components_config_path_arg,
+        declare_wheel_type_arg,  # wheel_type is used by controller_config_path
+        declare_controller_config_path_arg,
+        declare_namespace_arg,
+        declare_use_sim_arg,
+        declare_wheel_config_path_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        set_robot_description
+    ]
+
+    return LaunchDescription(actions)