added navigation test

toid_navigation/launch/mock_control_launch.py

@@ -20,15 +20,8 @@ def generate_launch_description():
         'toid_bot_description.urdf'
     )
 
-    starting_position = LaunchConfiguration("start_position")
     visualize = LaunchConfiguration("visualize")
 
-    starting_position_arg = DeclareLaunchArgument(
-        'start_position',
-        default_value='0 0 0 0 0 0',
-        description="Set initial robot position" 
-    )
-
     visualize_arg = DeclareLaunchArgument(
         'visualize',
         default_value='True',
@@ -39,7 +32,8 @@ def generate_launch_description():
         package='tf2_ros',
         executable='static_transform_publisher',
         name='map_to_odom_broadcaster',
-        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
+        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+        condition=IfCondition(LaunchConfiguration('visualize'))
     )
 
     robot_state_publisher = Node(
@@ -47,28 +41,28 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        parameters = [{'robot_description': Command(['xacro ', default_model_path])}]
+        parameters=[{'robot_description': Command(['xacro ', default_model_path])}]
     )
 
     controller_manager = Node(
         package='controller_manager',
         executable='ros2_control_node',
         output='screen',
-        parameters = [params]
+        parameters=[params]
     )
 
     joint_state_broadcaster = Node(
         package='controller_manager',
         executable='spawner',
         output='screen',
-        arguments= ["joint_state_broadcaster"]
+        arguments=["joint_state_broadcaster"]
     )
 
     diffbot_base_controller = Node(
         package='controller_manager',
         executable='spawner',
         output='both',
-        arguments= [
+        arguments=[
             "diffbot_base_controller",
             "-p",
             params,
@@ -89,7 +83,6 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        starting_position_arg,
         visualize_arg,
         odom_broadcast,
         robot_state_publisher,

toid_navigation/launch/navigation_launch.py

@@ -0,0 +1,112 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition, UnlessCondition 
+from launch.substitutions import  LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    pkg_share = FindPackageShare("").find('toid_navigation')
+    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
+    map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
+    launch_dir = os.path.join(pkg_share, 'launch')
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz')
+
+    position = "0.756 0.225 0 0 0 0".split(' ')
+    #position = "0 0 0 0 0 0".split(' ')
+    
+    visualize = LaunchConfiguration("visualize")
+    visualize_arg = DeclareLaunchArgument(
+        'visualize',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+    
+    toid_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'mock_control_launch.py')
+        ),
+        launch_arguments={
+            'visualize': 'False'
+        }.items()
+    )
+
+    set_position = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='map_to_odom_broadcaster',
+        arguments=position + ['map', 'odom'],
+        condition=IfCondition(visualize)
+    )
+
+    map_server = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map}]
+    )
+
+    planner_server = Node(
+        package='nav2_planner',
+        executable='planner_server',
+        name='planner_server',
+        output='screen',
+        parameters=[params]
+    )
+
+    controller_server = Node(
+        package='nav2_controller',
+        executable='controller_server',
+        name='controller_server',
+        output='screen',
+        parameters=[params]
+    )
+
+    bt_navigator = Node(
+        package='nav2_bt_navigator',
+        executable='bt_navigator',
+        name='bt_navigator',
+        output='screen',
+        parameters=[params]
+    )
+
+    behavior_server = Node(
+        package='nav2_behaviors',
+        executable='behavior_server',
+        name='behavior_server',
+        output='screen',
+        parameters=[params]
+    )
+
+    lifecycle_manager_node = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager',
+        output='screen',
+        parameters=[params]
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', default_rviz_config_path],
+        condition=IfCondition(visualize)
+    )
+
+    return LaunchDescription([
+        visualize_arg,
+        set_position,
+        rviz_node,
+        map_server,
+        bt_navigator,
+        behavior_server,
+        planner_server,
+        controller_server,
+        lifecycle_manager_node,
+        toid_control
+    ])