Modify move_group.launch.py with load_gripper args in launch. Need to…

… add rviz.launch.py as Husarion template launch file.

my_panda_bringup/launch/bringup.launch.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import (
+    PathJoinSubstitution,
+    LaunchConfiguration,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    #1
+    load_gripper = LaunchConfiguration("load_gripper")
+    declare_load_gripper_arg = DeclareLaunchArgument(
+        "load_gripper",
+        default_value="False",
+        description="Use Franka Gripper as end-effector if True",
+    )
+    #2
+    robot_ip = LaunchConfiguration("robot_ip")
+    declare_robot_ip_arg = DeclareLaunchArgument(
+        "robot_ip",
+        default_value="dont-care",
+        description=(
+            "Set the robot_ip to 'dont-care' for using the simulation."
+        ),
+    )
+    # 3
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="False",
+        description="Whether simulation is used",
+    )
+
+    #4
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    declare_use_fake_hardware_arg = DeclareLaunchArgument(
+        "use_fake_hardware",
+        default_value="False",
+        description="Wheter to use fake hardware",
+    )
+    #5
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    declare_fake_sensor_commands_arg = DeclareLaunchArgument(
+        "fake_sensor_commands",
+        default_value="False",
+        description="Wheter to use fake sensor commands",
+    )
+
+
+    # controller_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         PathJoinSubstitution(
+    #             [
+    #                 get_package_share_directory("rosbot_xl_manipulation_controller"),
+    #                 "launch",
+    #                 "controller.launch.py",
+    #             ]
+    #         )
+    #     ),
+    #     launch_arguments={
+    #         "manipulator_usb_port": manipulator_usb_port,
+    #         "manipulator_baud_rate": manipulator_baud_rate,
+    #         "joint1_limit_min": joint1_limit_min,
+    #         "joint1_limit_max": joint1_limit_max,
+    #         "antenna_rotation_angle": antenna_rotation_angle,
+    #         "mecanum": mecanum,
+    #         "use_sim": use_sim,
+    #     }.items(),
+    # )
+
+    moveit_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    get_package_share_directory("my_panda_moveit_config"),
+                    "launch",
+                    "move_group.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "load_gripper": load_gripper,
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "use_sim": use_sim,
+        }.items(),
+    )
+
+
+    actions = [
+        declare_load_gripper_arg,
+        declare_robot_ip_arg,
+        declare_use_sim_arg,
+        declare_use_fake_hardware_arg,
+        declare_fake_sensor_commands_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        # controller_launch,
+        moveit_launch
+    ]
+
+    return LaunchDescription(actions)

my_panda_gazebo/launch/simulation.launch.py

@@ -21,12 +21,46 @@
 
 def generate_launch_description():
 
+    #1
     load_gripper = LaunchConfiguration("load_gripper")
     declare_load_gripper_arg = DeclareLaunchArgument(
         "load_gripper",
         default_value="False",
         description="Use Franka Gripper as end-effector if True",
     )
+
+    #2
+    robot_ip = LaunchConfiguration("robot_ip")
+    declare_robot_ip_arg = DeclareLaunchArgument(
+        "robot_ip",
+        default_value="dont-care",
+        description=(
+            "Set the robot_ip to 'dont-care' for using the simulation."
+        ),
+    )
+    # 3 In simulation, this flag must be se to true
+    use_sim = LaunchConfiguration("use_sim")
+    declare_use_sim_arg = DeclareLaunchArgument(
+        "use_sim",
+        default_value="True",
+        description="Whether simulation is used",
+    )
+
+    #4
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    declare_use_fake_hardware_arg = DeclareLaunchArgument(
+        "use_fake_hardware",
+        default_value="False",
+        description="Wheter to use fake hardware",
+    )
+    #5
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    declare_fake_sensor_commands_arg = DeclareLaunchArgument(
+        "fake_sensor_commands",
+        default_value="False",
+        description="Wheter to use fake sensor commands",
+    )
+
     ## Spawn empty world in ignition-gazebo.
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -57,7 +91,7 @@ def generate_launch_description():
         ]
     )
 
-    # Load robot description content that will be used by 'gz_spawn_entity" node below.
+    # Load robot description content that will be used by 'gz_spawn_entity' node below.
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -71,7 +105,14 @@ def generate_launch_description():
             ),
             "hand:=",
             load_gripper,
-            "use_sim:=True",
+            "robot_ip:=",
+            robot_ip,
+            "use_sim:=",
+            use_sim,
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            "fake_sensor_commands:=",
+            fake_sensor_commands,
             "simulation_controllers_config_file:=",
             robot_controllers,
         ]
@@ -91,9 +132,34 @@ def generate_launch_description():
         output="screen",
     )
 
+    bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    get_package_share_directory("my_panda_bringup"),
+                    "launch",
+                    "bringup.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "load_gripper": load_gripper,
+            "robot_ip" :robot_ip,
+            "use_sim" :use_sim,
+            "use_fake_hardware" : use_fake_hardware,
+            "fake_sensor_commands" : fake_sensor_commands
+        }.items(),
+    )
+
     return LaunchDescription(
         [   declare_load_gripper_arg,
+            declare_robot_ip_arg,
+            declare_use_sim_arg,
+            declare_use_fake_hardware_arg,
+            declare_fake_sensor_commands_arg,
             gz_sim, ## Spawn empty world in ignition-gazebo.
-            gz_spawn_entity ## Spawn Franka Emika Robot on the table.
+            gz_spawn_entity, ## Spawn Franka Emika Robot on the table.
+            bringup_launch,
+            SetParameter(name="use_sim_time", value=True),
         ]
     )

my_panda_moveit_config/launch/move_group.launch.py

@@ -27,7 +27,7 @@
     FindExecutable,
     LaunchConfiguration,
 )
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetParameter
 import yaml
 
 
@@ -48,11 +48,15 @@ def generate_launch_description():
     robot_ip_parameter_name = 'robot_ip'
     use_fake_hardware_parameter_name = 'use_fake_hardware'
     fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_sim_parameter_name = 'use_sim'
+    load_gripper_parameter_name = 'load_gripper'
 
     robot_ip = LaunchConfiguration(robot_ip_parameter_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
     fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
-
+    use_sim = LaunchConfiguration(use_sim_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+
     # Command-line arguments
     db_arg = DeclareLaunchArgument(
         'db', default_value='False', description='Database flag'
@@ -69,13 +73,16 @@ def generate_launch_description():
             FindExecutable(name='xacro'),
             ' ',
             franka_xacro_file,
-            ' hand:=true',
+            ' hand:=',
+            load_gripper,
             ' robot_ip:=',
             robot_ip,
             ' use_fake_hardware:=',
             use_fake_hardware,
             ' fake_sensor_commands:=',
             fake_sensor_commands,
+            'use_sim:=',
+             use_sim
         ]
     )
 
@@ -87,7 +94,7 @@ def generate_launch_description():
         'panda_arm.srdf.xacro',
     )
     robot_description_semantic_config = Command(
-        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true']
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper]
     )
     robot_description_semantic = {
         'robot_description_semantic': robot_description_semantic_config
@@ -109,5 +116,6 @@ def generate_launch_description():
         [
             db_arg,
             run_move_group_node,
+            SetParameter(name="use_sim_time", value=use_sim)
         ]
     )

my_panda_moveit_config/launch/moveit.launch.py

@@ -26,6 +26,7 @@
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
 import yaml
 
 
@@ -44,11 +45,15 @@ def generate_launch_description():
     robot_ip_parameter_name = 'robot_ip'
     use_fake_hardware_parameter_name = 'use_fake_hardware'
     fake_sensor_commands_parameter_name = 'fake_sensor_commands'
-
+    load_gripper_parameter_name = 'load_gripper'
+    use_sim_parameter_name = 'use_sim'
+
     robot_ip = LaunchConfiguration(robot_ip_parameter_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
     fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
-
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_sim = LaunchConfiguration(use_sim_parameter_name)
+
     # Command-line arguments
 
     db_arg = DeclareLaunchArgument(
@@ -59,7 +64,7 @@ def generate_launch_description():
     franka_xacro_file = os.path.join(get_package_share_directory('my_panda_description'), 'robots',
                                      'panda_arm.urdf.xacro')
     robot_description_config = Command(
-        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true',
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper,
          ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
          ' fake_sensor_commands:=', fake_sensor_commands])
 
@@ -69,7 +74,7 @@ def generate_launch_description():
                                               'srdf',
                                               'panda_arm.srdf.xacro')
     robot_description_semantic_config = Command(
-        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true']
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper]
     )
     robot_description_semantic = {
         'robot_description_semantic': robot_description_semantic_config
@@ -221,16 +226,30 @@ def generate_launch_description():
         default_value='false',
         description="Fake sensor commands. Only valid when '{}' is true".format(
             use_fake_hardware_parameter_name))
+    load_gripper_arg = DeclareLaunchArgument(
+        load_gripper_parameter_name,
+        default_value='false',
+        description="Load panda flange gripper".format(
+            load_gripper_parameter_name))
+    use_sim_arg = DeclareLaunchArgument(
+        use_sim_parameter_name,
+        default_value='false',
+        description="Use sim  for simulation".format(
+            use_sim_parameter_name))
+
     gripper_launch_file = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([PathJoinSubstitution(
             [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
         launch_arguments={'robot_ip': robot_ip,
                           use_fake_hardware_parameter_name: use_fake_hardware}.items(),
+        condition=IfCondition(load_gripper),
     )
     return LaunchDescription(
         [robot_arg,
          use_fake_hardware_arg,
          fake_sensor_commands_arg,
+         load_gripper_arg,
+         use_sim_arg,
          db_arg,
          rviz_node,
          robot_state_publisher,