MoveIt Disable and Generate

clearpath_common/launch/platform.launch.py

@@ -66,10 +66,18 @@ def generate_launch_description():
         description='Robot namespace'
     )
 
+    arg_enable_ekf = DeclareLaunchArgument(
+        'enable_ekf',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Enable localization via EKF node'
+    )
+
     # Launch Configurations
     setup_path = LaunchConfiguration('setup_path')
     use_sim_time = LaunchConfiguration('use_sim_time')
     namespace = LaunchConfiguration('namespace')
+    enable_ekf = LaunchConfiguration('enable_ekf')
 
     # Launch files
     launch_file_platform_description = PathJoinSubstitution([
@@ -104,45 +112,49 @@ def generate_launch_description():
             IncludeLaunchDescription(
               PythonLaunchDescriptionSource(launch_file_platform_description),
               launch_arguments=[
-                  ('setup_path', setup_path),
-                  ('use_sim_time', use_sim_time),
-                  ('namespace', namespace),
+                ('setup_path', setup_path),
+                ('use_sim_time', use_sim_time),
+                ('namespace', namespace),
               ]
             ),
 
             # Launch clearpath_control/control.launch.py which is just robot_localization.
             IncludeLaunchDescription(
               PythonLaunchDescriptionSource(launch_file_control),
               launch_arguments=[
-                  ('setup_path', setup_path),
-                  ('use_sim_time', use_sim_time),
+                ('setup_path', setup_path),
+                ('use_sim_time', use_sim_time),
               ]
             ),
 
             # Launch localization (ekf node)
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_localization),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                  ('enable_ekf', enable_ekf)
+                ]
             ),
 
             # Launch clearpath_control/teleop_base.launch.py which is various ways to tele-op
             # the robot but does not include the joystick. Also, has a twist mux.
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_teleop_base),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                ]
             ),
 
             # Launch clearpath_control/teleop_joy.launch.py which is tele-operation using a
             # physical joystick.
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(launch_file_teleop_joy),
                 launch_arguments=[
-                    ('setup_path', setup_path),
-                    ('use_sim_time', use_sim_time)]
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time),
+                ]
             ),
         ]
     )
@@ -151,5 +163,6 @@ def generate_launch_description():
     ld.add_action(arg_setup_path)
     ld.add_action(arg_use_sim_time)
     ld.add_action(arg_namespace)
+    ld.add_action(arg_enable_ekf)
     ld.add_action(group_platform_action)
     return ld

clearpath_control/launch/localization.launch.py

@@ -34,22 +34,29 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
 
     # Launch Configurations
+    enable_ekf = LaunchConfiguration('enable_ekf')
     setup_path = LaunchConfiguration('setup_path')
     use_sim_time = LaunchConfiguration('use_sim_time')
 
     # Launch Arguments
+    arg_enable_ekf = DeclareLaunchArgument(
+        'enable_ekf',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Enable localization via EKF node'
+    )
     arg_setup_path = DeclareLaunchArgument(
         'setup_path',
         default_value='/etc/clearpath/'
     )
-
     arg_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         choices=['true', 'false'],
@@ -69,20 +76,22 @@ def generate_launch_description():
 
     # Localization
     node_localization = Node(
-            package='robot_localization',
-            executable='ekf_node',
-            name='ekf_node',
-            output='screen',
-            parameters=[config_localization],
-            remappings=[
-              ('odometry/filtered', 'platform/odom/filtered'),
-              ('/diagnostics', 'diagnostics'),
-              ('/tf', 'tf'),
-              ('/tf_static', 'tf_static'),
-            ]
-        )
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_node',
+        output='screen',
+        parameters=[config_localization],
+        remappings=[
+            ('odometry/filtered', 'platform/odom/filtered'),
+            ('/diagnostics', 'diagnostics'),
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ],
+        condition=IfCondition(enable_ekf),
+    )
 
     ld = LaunchDescription()
+    ld.add_action(arg_enable_ekf)
     ld.add_action(arg_setup_path)
     ld.add_action(arg_use_sim_time)
     ld.add_action(node_localization)

clearpath_generator_common/clearpath_generator_common/launch/generator.py

@@ -68,9 +68,10 @@ def __init__(self,
             name='platform',
             package=self.pkg_clearpath_common,
             args=[
-              ('setup_path', self.setup_path),
-              ('use_sim_time', 'false'),
-              ('namespace', self.namespace),
+                ('setup_path', self.setup_path),
+                ('use_sim_time', 'false'),
+                ('namespace', self.namespace),
+                ('enable_ekf', str(self.clearpath_config.platform.enable_ekf).lower()),
             ])
 
         self.manipulators_launch_file = LaunchFile(

clearpath_manipulators/launch/manipulators.launch.py

@@ -38,6 +38,7 @@
     IncludeLaunchDescription,
     TimerAction
 )
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
@@ -72,10 +73,18 @@ def generate_launch_description():
         description='Robot namespace'
     )
 
+    arg_launch_moveit = DeclareLaunchArgument(
+        'launch_moveit',
+        choices=['true', 'false'],
+        default_value='false',
+        description='Launch MoveIt'
+    )
+
     # Launch Configurations
     setup_path = LaunchConfiguration('setup_path')
     use_sim_time = LaunchConfiguration('use_sim_time')
     namespace = LaunchConfiguration('namespace')
+    launch_moveit = LaunchConfiguration('launch_moveit')
 
     # Launch files
     launch_file_manipulators_description = PathJoinSubstitution([
@@ -120,6 +129,7 @@ def generate_launch_description():
     # Launch MoveIt
     moveit_node_action = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(launch_file_moveit),
+        condition=IfCondition(launch_moveit),
         launch_arguments=[
             ('setup_path', setup_path),
             ('use_sim_time', use_sim_time)
@@ -135,6 +145,7 @@ def generate_launch_description():
     ld.add_action(arg_setup_path)
     ld.add_action(arg_use_sim_time)
     ld.add_action(arg_namespace)
+    ld.add_action(arg_launch_moveit)
     ld.add_action(group_manipulators_action)
     ld.add_action(moveit_delayed)
     return ld

clearpath_platform_description/urdf/do100/do100.urdf.xacro

@@ -4,6 +4,7 @@
   <xacro:arg name="namespace" default="" />
   <xacro:arg name="is_sim" default="false" />
   <xacro:arg name="gazebo_controllers" default="$(find clearpath_control)/config/do100/control.yaml"/>
+  <xacro:arg name="use_platform_controllers" default="true"/>
 
   <!-- Included URDF/Xacro Files -->
   <xacro:include filename="$(find clearpath_platform_description)/urdf/do100/wheels/wheel.urdf.xacro"/>
@@ -185,36 +186,38 @@
     <xacro:include filename="$(find clearpath_platform_description)/urdf/generic/gazebo.urdf.xacro"/>
 
     <!-- ROS2 controls -->
-    <ros2_control name="do100_hardware" type="system">
-      <hardware>
-        <xacro:if value="$(arg is_sim)">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
-        </xacro:if>
-        <xacro:unless value="$(arg is_sim)">
-          <plugin>clearpath_hardware_interfaces/PumaHardware</plugin>
-        </xacro:unless>
-      </hardware>
-      <joint name="front_left_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="front_right_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="rear_left_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="rear_right_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-    </ros2_control>
+    <xacro:if value="$(arg use_platform_controllers)">
+      <ros2_control name="do100_hardware" type="system">
+        <hardware>
+          <xacro:if value="$(arg is_sim)">
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          </xacro:if>
+          <xacro:unless value="$(arg is_sim)">
+            <plugin>clearpath_hardware_interfaces/PumaHardware</plugin>
+          </xacro:unless>
+        </hardware>
+        <joint name="front_left_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="front_right_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="rear_left_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="rear_right_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+      </ros2_control>
+    </xacro:if>
   </xacro:macro>
 
   <!-- Joint State Publisher -->

clearpath_platform_description/urdf/do150/do150.urdf.xacro

@@ -4,6 +4,7 @@
   <xacro:arg name="namespace" default="" />
   <xacro:arg name="is_sim" default="false" />
   <xacro:arg name="gazebo_controllers" default="$(find clearpath_control)/config/do150/control.yaml"/>
+  <xacro:arg name="use_platform_controllers" default="true"/>
 
   <!-- Included URDF/Xacro Files -->
   <xacro:include filename="$(find clearpath_platform_description)/urdf/do100/do100.urdf.xacro"/>

clearpath_platform_description/urdf/r100/r100.urdf.xacro

@@ -4,6 +4,7 @@
   <xacro:arg name="namespace" default="" />
   <xacro:arg name="is_sim" default="false" />
   <xacro:arg name="gazebo_controllers" default="$(find clearpath_control)/config/r100/control.yaml"/>
+  <xacro:arg name="use_platform_controllers" default="true"/>
 
   <!-- Includes -->
   <xacro:include filename="$(find clearpath_platform_description)/urdf/r100/wheels/wheel.urdf.xacro"/>
@@ -126,7 +127,7 @@
         <geometry>
           <mesh filename="package://clearpath_platform_description/meshes/r100/end-cover.stl" />
         </geometry>
-        <material name="clearpath_black" />
+        <material name="clearpath_red" />
       </visual>
     </link>
 
@@ -296,36 +297,38 @@
     <xacro:include filename="$(find clearpath_platform_description)/urdf/generic/gazebo.urdf.xacro"/>
 
     <!-- ROS2 controls -->
-    <ros2_control name="r100_hardware" type="system">
-      <hardware>
-        <xacro:if value="$(arg is_sim)">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
-        </xacro:if>
-        <xacro:unless value="$(arg is_sim)">
-          <plugin>clearpath_hardware_interfaces/PumaHardware</plugin>
-        </xacro:unless>
-      </hardware>
-      <joint name="front_left_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="front_right_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="rear_left_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="rear_right_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-    </ros2_control>
+    <xacro:if value="$(arg use_platform_controllers)">
+      <ros2_control name="r100_hardware" type="system">
+        <hardware>
+          <xacro:if value="$(arg is_sim)">
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          </xacro:if>
+          <xacro:unless value="$(arg is_sim)">
+            <plugin>clearpath_hardware_interfaces/PumaHardware</plugin>
+          </xacro:unless>
+        </hardware>
+        <joint name="front_left_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="front_right_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="rear_left_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="rear_right_wheel_joint">
+          <command_interface name="velocity"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+      </ros2_control>
+    </xacro:if>
 
   </xacro:macro>
 </robot>