diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e9a232d --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +*.py[cod] +.obj-x86_64-linux-gnu/ +debian/ diff --git a/clearpath_platform/launch/platform.launch.py b/clearpath_platform/launch/platform.launch.py index a4b5e0b..d8e5389 100644 --- a/clearpath_platform/launch/platform.launch.py +++ b/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,7 +185,6 @@ 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 @@ -124,36 +192,33 @@ def generate_launch_description(): 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