Merge pull request #433 from husarion/ros2-add-rviz-launch

Ros2 add RViz launch

README.md

@@ -108,6 +108,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | ✅   | ✅   | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj)                                                                                                                    |
 | ✅   | ✅   | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml)                                                                                                                                                                |
 | ✅   | ✅   | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                    |
+| ❌   | ✅   | `use_rviz`                   | Run RViz simultaneously. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | ✅   | ✅   | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                              |
 | ✅   | ✅   | `user_led_animations_path`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                    |
 | ✅   | ✅   | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                          |

husarion_ugv_gazebo/launch/simulation.launch.py

@@ -16,6 +16,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -45,6 +46,14 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
+    use_rviz = LaunchConfiguration("use_rviz")
+    declare_use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz",
+        default_value="True",
+        description="Run RViz simultaneously.",
+        choices=["True", "true", "False", "false"],
+    )
+
     namespaced_gz_gui = ReplaceString(
         source_file=gz_gui,
         replacements={"{namespace}": namespace},
@@ -59,6 +68,19 @@ def generate_launch_description():
         launch_arguments={"gz_gui": namespaced_gz_gui, "gz_log_level": "1"}.items(),
     )
 
+    rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_description"),
+                    "launch",
+                    "rviz.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(use_rviz),
+    )
+
     simulate_robots = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -74,9 +96,11 @@ def generate_launch_description():
     actions = [
         declare_gz_gui,
         declare_namespace_arg,
+        declare_use_rviz_arg,
         # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo)
         SetUseSimTime(True),
         gz_sim,
+        rviz_launch,
         simulate_robots,
     ]
 

lynx_description/urdf/gazebo.urdf.xacro

@@ -42,12 +42,15 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
+          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>drive_controller/transition_event:=drive_controller/_transition_event</remapping>
           <remapping>gz_ros2_control/e_stop:=hardware/e_stop</remapping>
           <remapping>gz_ros2_control/e_stop_reset:=hardware/e_stop_reset</remapping>
           <remapping>gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger</remapping>
           <remapping>imu_broadcaster/imu:=imu/data</remapping>
-          <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
-          <remapping>drive_controller/odom:=odometry/wheels</remapping>
+          <remapping>imu_broadcaster/transition_event:=imu_broadcaster/_transition_event</remapping>
+          <remapping>joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event</remapping>
         </ros>
       </plugin>
     </gazebo>

panther_description/launch/rviz.launch.py

@@ -0,0 +1,79 @@
+#!/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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import (
+    EnvironmentVariable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+from nav2_common.launch import ReplaceString
+
+
+def generate_launch_description():
+
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    rviz_config = LaunchConfiguration("rviz_config")
+    declare_rviz_config_arg = DeclareLaunchArgument(
+        "rviz_config",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_description"), "rviz", "husarion_ugv.rviz"]
+        ),
+        description="RViz configuration file.",
+    )
+
+    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"],
+    )
+
+    ns_ext = PythonExpression(["'' if '", namespace, "' else '", namespace, "' + '/'"])
+
+    rviz_config = ReplaceString(
+        source_file=rviz_config,
+        replacements={"<robot_namespace>/": ns_ext, "<robot_namespace>": namespace},
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        namespace=namespace,
+        arguments=["-d", rviz_config],
+    )
+
+    actions = [
+        declare_namespace_arg,
+        declare_rviz_config_arg,
+        declare_use_sim_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        rviz_node,
+    ]
+
+    return LaunchDescription(actions)

panther_description/package.xml

@@ -20,8 +20,10 @@
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>nav2_common</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros_components_description</exec_depend>
+  <exec_depend>rviz</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <export>