Merge pull request #6 from clearpathrobotics/simulator

Simulator support

.gitignore

@@ -0,0 +1,4 @@
+__pycache__/
+*.py[cod]
+.obj-x86_64-linux-gnu/
+debian/

clearpath_platform/launch/platform.launch.py

@@ -27,10 +27,21 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, GroupAction, IncludeLaunchDescription, LogInfo
-from launch.conditions import LaunchConfigurationEquals
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo
+)
+from launch.conditions import LaunchConfigurationEquals, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import EnvironmentVariable, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import (
+    EnvironmentVariable,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -49,6 +60,11 @@ def generate_launch_description():
         default_value='a200'
     )
 
+    arg_setup_path = DeclareLaunchArgument(
+        'setup_path',
+        default_value='/etc/clearpath/'
+    )
+
     arg_imu_filter_config = DeclareLaunchArgument(
         'imu_filter_config',
         default_value=PathJoinSubstitution([
@@ -61,37 +77,90 @@ def generate_launch_description():
         default_value='ps4'
     )
 
+    arg_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time',
+        choices=['true', 'false'],
+        default_value='false',
+        description='Use simulation time'
+    )
+
+    # Launch Configurations
     platform_model = LaunchConfiguration('platform_model')
+    setup_path = LaunchConfiguration('setup_path')
     config_imu_filter = LaunchConfiguration('imu_filter_config')
     joy_type = LaunchConfiguration('joy_type')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    # Launch files
+    launch_file_platform_description = PathJoinSubstitution([
+      pkg_clearpath_platform_description,
+      'launch',
+      'description.launch.py'])
+
+    launch_file_control = PathJoinSubstitution([
+      pkg_clearpath_control,
+      'launch',
+      'control.launch.py'])
+
+    launch_file_localization = PathJoinSubstitution([
+      pkg_clearpath_control,
+      'launch',
+      'localization.launch.py'])
+
+    launch_file_teleop_base = PathJoinSubstitution([
+      pkg_clearpath_control,
+      'launch',
+      'teleop_base.launch.py'])
+
+    launch_file_teleop_joy = PathJoinSubstitution([
+      pkg_clearpath_control,
+      'launch',
+      'teleop_joy.launch.py'])
 
     log_platform_model = LogInfo(msg=["Launching Clearpath platform model: ", platform_model])
 
     group_platform_action = GroupAction(
         actions=[
+            IncludeLaunchDescription(
+              PythonLaunchDescriptionSource(launch_file_platform_description),
+              launch_arguments=[
+                  ('setup_path', setup_path),
+                  ('use_sim_time', use_sim_time)]),
+
             # Launch clearpath_control/control.launch.py which is just robot_localization.
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(PathJoinSubstitution(
-                  [pkg_clearpath_control, 'launch', 'control.launch.py']))
+                PythonLaunchDescriptionSource(launch_file_control),
+                launch_arguments=[
+                    ('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)]
             ),
 
             # 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(PathJoinSubstitution(
-                  [pkg_clearpath_control, 'launch', 'teleop_base.launch.py'])),
+                PythonLaunchDescriptionSource(launch_file_teleop_base),
+                launch_arguments=[('use_sim_time', use_sim_time)]
             ),
 
             # Launch clearpath_control/teleop_joy.launch.py which is tele-operation using a physical joystick.
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(PathJoinSubstitution(
-                  [pkg_clearpath_control, 'launch', 'teleop_joy.launch.py'])),
-                launch_arguments=[('joy_type', joy_type)]
-            )
+                PythonLaunchDescriptionSource(launch_file_teleop_joy),
+                launch_arguments=[
+                    ('joy_type', joy_type),
+                    ('use_sim_time', use_sim_time)
+                ]
+            ),
         ]
     )
 
-
     # Group for actions needed for the Clearpath Jackal J100 platform.
     group_j100_action = GroupAction(
         condition=LaunchConfigurationEquals('platform_model', 'j100'),
@@ -116,44 +185,40 @@ def generate_launch_description():
                   ('imu/mag', 'platform/sensors/imu_0/magnetic_field'),
                   ('imu/data', 'platform/sensors/imu_0/data')
                 ],
-                condition=LaunchConfigurationEquals('platform_model', 'j100')
             ),
 
             # MicroROS Agent
             Node(
                 package='micro_ros_agent',
                 executable='micro_ros_agent',
                 arguments=['serial', '--dev', '/dev/clearpath/j100'],
-                output='screen'),
+                output='screen',
+                condition=UnlessCondition(use_sim_time)),
 
             # Set ROS_DOMAIN_ID
             ExecuteProcess(
                 cmd=[
                     ['export ROS_DOMAIN_ID=0;'],
                     [FindExecutable(name='ros2'),
-                    ' service call platform/mcu/set_domain_id ',
-                    ' clearpath_platform_msgs/srv/SetDomainId ',
-                    '"domain_id: ',
-                    EnvironmentVariable('ROS_DOMAIN_ID', default_value='0'),
-                    '"']
+                     ' service call platform/mcu/set_domain_id ',
+                     ' clearpath_platform_msgs/srv/SetDomainId ',
+                     '"domain_id: ',
+                     EnvironmentVariable('ROS_DOMAIN_ID', default_value='0'),
+                     '"']
                 ],
                 shell=True,
+                condition=UnlessCondition(use_sim_time)
             )
         ]
     )
 
-    launch_description = IncludeLaunchDescription(
-            PathJoinSubstitution([
-                pkg_clearpath_platform_description,
-                'launch',
-                'description.launch.py']))
-
     ld = LaunchDescription()
     ld.add_action(arg_platform_model)
+    ld.add_action(arg_setup_path)
     ld.add_action(arg_imu_filter_config)
     ld.add_action(arg_joy_type)
+    ld.add_action(arg_use_sim_time)
     ld.add_action(log_platform_model)
-    ld.add_action(launch_description)
     ld.add_action(group_platform_action)
     ld.add_action(group_j100_action)
     return ld