Added rbs_gym package for RL & multi-robot launch setup

rbs_bringup/launch/rbs_robot.launch.py

@@ -14,12 +14,14 @@ from launch_ros.substitutions import FindPackageShare
 import xacro
 import os
 from ament_index_python.packages import get_package_share_directory
+from rbs_launch_utils.launch_common import load_yaml
 
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
     # General arguments
+    launch_rviz = LaunchConfiguration("launch_rviz")
     with_gripper_condition = LaunchConfiguration("with_gripper")
     controllers_file = LaunchConfiguration("controllers_file")
     cartesian_controllers = LaunchConfiguration("cartesian_controllers")
@@ -44,14 +46,20 @@ def launch_setup(context, *args, **kwargs):
     pitch = LaunchConfiguration("pitch")
     yaw = LaunchConfiguration("yaw")
     namespace = LaunchConfiguration("namespace")
+    multi_robot = LaunchConfiguration("multi_robot")
     robot_name = robot_name.perform(context)
     namespace = namespace.perform(context)
     robot_type = robot_type.perform(context)
     description_package = description_package.perform(context)
     description_file = description_file.perform(context)
     controllers_file = controllers_file.perform(context)
+    multi_robot = multi_robot.perform(context)
 
-    # remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+    remappings = []
+    if multi_robot == "true":
+        remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
+
+    controllers_file = os.path.join(get_package_share_directory(description_package), "config", controllers_file)
 
     xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file)
     robot_description_doc = xacro.process_file(
@@ -67,7 +75,6 @@ def launch_setup(context, *args, **kwargs):
             "roll": roll.perform(context),
             "pitch": pitch.perform(context),
             "yaw": yaw.perform(context)
-            #TODO: add rotation and add probably via dict
         }
     )
 
@@ -83,7 +90,7 @@ def launch_setup(context, *args, **kwargs):
                 [FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
             ),
             " ",
-            "name:=",robot_name," ",
+            "name:=",robot_type," ",
             "with_gripper:=",with_gripper_condition, " ",
             "gripper_name:=", gripper_name, " ",
         ]
@@ -93,12 +100,19 @@ def launch_setup(context, *args, **kwargs):
     robot_description_kinematics = PathJoinSubstitution(
         [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
     )
+
+    # kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
+
+    robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics}
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
+    )
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         namespace=namespace,
         output="both",
-        # remappings=remappings,
+        remappings=remappings,
         parameters=[{"use_sim_time": use_sim_time}, robot_description],
     )
 
@@ -121,6 +135,19 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics
+        ],
+        condition=IfCondition(launch_rviz))
+
     control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
             PathJoinSubstitution([
@@ -209,9 +236,9 @@ def launch_setup(context, *args, **kwargs):
         control,
         moveit,
         skills,
-        task_planner,
-        perception,
-        # rviz
+        # task_planner,
+        # perception,
+        rviz
     ]
     return nodes_to_start
 
@@ -230,7 +257,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
+            default_value="rbs_arm0_controllers.yaml",
             description="YAML file with the controllers configuration.",
         )
     )
@@ -310,7 +337,7 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument("launch_rviz",
-                              default_value="true",
+                              default_value="false",
                               description="Launch RViz?")
     )
     declared_arguments.append(
@@ -401,6 +428,29 @@ def generate_launch_description():
                               default_value="0.0",
                               description="Position of robot in world by Z")
     )
+    declared_arguments.append(
+        DeclareLaunchArgument("roll", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("pitch", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("yaw", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "multi_robot",
+            default_value="false",
+            description="Flag if you use multi robot setup"
+        )
+    )
 
 
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])